{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import time\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "import math\n",
    "plt.style.use({'figure.figsize':(10, 10)})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "Epsilon_start=1\n",
    "Epsilon_final=0.01\n",
    "Robot_radium=40\n",
    "Decay_rate=0.000001#he dacaying rate of the Epsilon, the range of the epsilon is 0.01-1, initially it is 1.\n",
    "Action_times=0 #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Velocity_tripod=0.289*Robot_radium\n",
    "Beta=0.9\n",
    "Alpha=0.2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style>\n",
       "    .dataframe thead tr:only-child th {\n",
       "        text-align: right;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: left;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>Up</th>\n",
       "      <th>Left_45D</th>\n",
       "      <th>Right_45D</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>L60D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L180D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L250D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L300D0/30</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>L60D30/60</th>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "      <td>0.0</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>"
      ],
      "text/plain": [
       "            Up  Left_45D  Right_45D\n",
       "L60D0/30   0.0       0.0        0.0\n",
       "L180D0/30  0.0       0.0        0.0\n",
       "L250D0/30  0.0       0.0        0.0\n",
       "L300D0/30  0.0       0.0        0.0\n",
       "L60D30/60  0.0       0.0        0.0"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Q_table2_states=np.array(['L60D0/30','L180D0/30','L250D0/30','L300D0/30',\n",
    "                'L60D30/60','L180D30/60','L250D30/60','L300D30/60',\n",
    "                 'L60D60/90','L180D60/90','L250D60/90','L300D60/90',\n",
    "                'L60D90/120','L180D90/120','L250D90/120','L300D90/120',\n",
    "                'L60D120/150','L180D120/150','L250D120/150','L300D120/150',\n",
    "                'L60D150/180','L180D150/180','L250D150/180','L300D150/180',\n",
    "                'L60D180/210','L180D180/210','L250D180/210','L300D180/210',\n",
    "                'L60D210/240','L180D210/240','L250D210/240','L300D210/240',\n",
    "                'L60D240/270','L180D240/270','L250D240/270','L300D240/270',\n",
    "                'L60D270/300','L180D270/300','L250D270/300','L300D270/300',\n",
    "                'L60D300/330','L180D300/330','L250D300/330','L300D300/330',\n",
    "                 'L60D330/360','L180D330/360','L250D330/360','L300D330/360'])\n",
    "Q_table2_action=np.array(['Up','Left_45D','Right_45D'])\n",
    "Q_table2_actions_length=len(Q_table2_action)\n",
    "Q_table2_states_length=len(Q_table2_states)\n",
    "Q_table2_real=np.zeros((Q_table2_states_length,Q_table2_actions_length))\n",
    "Q_table2_real=pd.DataFrame(Q_table2_real,columns=Q_table2_action,index=Q_table2_states)\n",
    "Q_table2_real.head()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Set_destination(x,y):\n",
    "    return x,y\n",
    "# Destination_x,Destination_y=Set_destination(900,900)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAlsAAAJCCAYAAAD3HAIiAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nYJDYAgAYJLYAAAb9f3n35cMwEAlBAAAAAElFTkSuQmCC\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x221f1f1b240>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "#Draw_map2 funcion is used to train the first Q-Table2, which allows it to arrive to the destination\n",
    "def Draw_map(Destination_x,Destination_y):\n",
    "    im = Image.new(\"RGB\", size=(1000,1000),color=(0,0,0)) \n",
    "    draw = ImageDraw.Draw(im,mode='RGB')\n",
    "    draw.rectangle((40,40,960,960),(255,255,255),(255,255,255))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0), (255,0,0)) \n",
    "    return im\n",
    "im=Draw_map(900,900)\n",
    "plt.imshow(im)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Initial_Q_Table2(LengthOfActions,LengthOfStates):\n",
    "    Q_table2=np.zeros((LengthOfStates,LengthOfActions))\n",
    "    print('***********************************************************')\n",
    "    print(\"Succeed to initialize Q-table2!\")\n",
    "    print('***********************************************************')\n",
    "    return Q_table2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Robot_destination(Destination_x,Destination_y,Current_x,Current_y,Current_angle):\n",
    "    #Left-negative,180-360 |||| Right-positive,0-180\n",
    "    Delta_x=Destination_x-Current_x\n",
    "    Delta_y=Destination_y-Current_y\n",
    "    Distance=np.sqrt(Delta_x*Delta_x+Delta_y*Delta_y)\n",
    "    Direct_angle=(np.arctan(Delta_y/Delta_x))*180/np.pi\n",
    "    if Delta_x<0:\n",
    "        Direct_angle=Direct_angle+180\n",
    "    True_angle=(Current_angle-Direct_angle)%360\n",
    "#     print(true_angle)\n",
    "    if True_angle>180:\n",
    "        return 360-True_angle,'right',Distance\n",
    "    else:\n",
    "        return True_angle,'left',Distance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Is_Crash function, in the second map, is used to judge if the robot collides with the black frame\n",
    "def Is_Crash(Current_x,Current_y,im):\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,50,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "        if Crash==True:\n",
    "                break\n",
    "    return Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Is_arrive(Current_x,Current_y,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Distance=np.arange(0,40,5)\n",
    "    for i in Distance:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "            if (im.getpixel((x,y)))==(255,0,0):\n",
    "                Arrive=True\n",
    "                break\n",
    "        if Arrive==True or Crash==True:\n",
    "                break\n",
    "    return Arrive\n",
    "# print(Is_arrive(500,500))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Random_start function, in the second map, is used to set the beginning point in the white map.\n",
    "def Random_start(im):\n",
    "    Angle=0\n",
    "    x,y=np.random.random(2)*1000\n",
    "    while(Is_Crash(x,y,im)==True or Is_arrive(x,y,im)==True):\n",
    "#         print('Boom')\n",
    "        x,y=np.random.random(2)*1000\n",
    "    return x,y,Angle"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Output_state_index(True_angle,Left_right,Distance):\n",
    "    if 0<=Distance<60:\n",
    "        Distance_level=0\n",
    "    elif 60<=Distance<180:\n",
    "        Distance_level=1\n",
    "    elif 180<=Distance<250:\n",
    "        Distance_level=2\n",
    "    else:\n",
    "        Distance_level=3\n",
    "    Angle_level=True_angle//30\n",
    "    \n",
    "    if Left_right=='right':\n",
    "        Left_right_level=1\n",
    "        State_number=int(Distance_level+24*Left_right_level+(5-Angle_level)*4)\n",
    "    elif Left_right=='left':\n",
    "        Left_right_level=0\n",
    "        Angle_level=True_angle//30\n",
    "        State_number=int(Distance_level+24*Left_right_level+Angle_level*4)\n",
    "    return State_number,Distance_level,Angle_level\n",
    "# State_Number,_,_=Output_state_index(20,'right',299) #180 belongs to 180-210,''right' does not have 180\n",
    "# Q_table2_states[State_Number]\n",
    "    \n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Choose_action2(Q_Table,Current_state,Action_times):\n",
    "    Epsilon=Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_rate*Action_times)\n",
    "    State_action=Q_Table[Current_state,:]\n",
    "    if(np.random.random()<Epsilon or np.all(State_action==[0])):\n",
    "        Next_action=np.random.randint(Q_table2_actions_length)\n",
    "    else:\n",
    "        Next_action=np.argmax(State_action)\n",
    "    return Next_action\n",
    "# print(Choose_action2(Q_Table2,20,0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Output_next_state(Current_x,Current_y,Current_angle,Destination_x,Destination_y,Action,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    if Action==0:\n",
    "        Next_x=Current_x+Velocity_tripod*np.cos(Current_angle/180*np.pi)\n",
    "        Next_y=Current_y+Velocity_tripod*np.sin(Current_angle/180*np.pi)\n",
    "        Next_angle=Current_angle\n",
    "        if Is_Crash(Next_x,Next_y,im)==True:\n",
    "            Crash=True\n",
    "        if Is_arrive(Next_x,Next_y,im)==True:\n",
    "            Arrive=True\n",
    "    elif Action==1:\n",
    "        Next_x=Current_x\n",
    "        Next_y=Current_y\n",
    "        Next_angle=Current_angle-45\n",
    "    elif Action==2:\n",
    "        Next_x=Current_x\n",
    "        Next_y=Current_y\n",
    "        Next_angle=Current_angle+45        \n",
    "    True_angle,Left_right,Distance=Robot_destination(Destination_x,Destination_y,Next_x,Next_y,Next_angle)\n",
    "    State_number,Distance_level,Angle_level=Output_state_index(True_angle,Left_right,Distance)\n",
    "    if Arrive==False:\n",
    "        Reward=Distance_level*-0.5-Angle_level\n",
    "    elif Arrive==True:\n",
    "        Reward=50\n",
    "    return Next_x,Next_y,Next_angle,Reward,State_number,Arrive,Crash\n",
    "#Next_state_F2(600,600,50,1800,1800,1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Movement_plot(Vec_x,Vec_y,Destination_x,Destination_y):\n",
    "    im=Image.new(\"RGB\", size=(1000,1000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im,mode='RGB')\n",
    "    draw.rectangle((40,40,960,960),255,255)\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20),(255,0,0),(255,0,0))\n",
    "    for i,j in zip(Vec_x,Vec_y):\n",
    "        draw.ellipse((i-40,j-40, i+40,j+40),150,150) \n",
    "    return im\n",
    "\n",
    "# plt.imshow(im)\n",
    "# plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "***********************************************************\n",
      "Succeed to initialize Q-table2!\n",
      "***********************************************************\n"
     ]
    }
   ],
   "source": [
    "Q_Table2=Initial_Q_Table2(Q_table2_actions_length,Q_table2_states_length)\n",
    "global Epoche\n",
    "Epoche=0\n",
    "global Action_times #Rigister the totality of the times of selecting actions, including the random selections and selection based on Q_Table\n",
    "Action_times=0\n",
    "global Epoche_action_interval"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "**********************************\n",
      "last_x=875.282180, last_y=841.519466,last_a=90.000000\n",
      "x=875.282180 y=853.079466 True_angle=27.780340 Des_Ro_dis=53.033076 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=62.219660 Robot_a=90\n",
      "0.984220771876\n",
      "16067\n",
      "False_epoche=1\n",
      "**********************************\n",
      "last_x=859.957341, last_y=861.984632,last_a=45.000000\n",
      "x=868.131496 y=870.158786 True_angle=1.881607 Des_Ro_dis=43.658901 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=43.118393 Robot_a=45\n",
      "0.975780848076\n",
      "24768\n",
      "False_epoche=2\n",
      "**********************************\n",
      "last_x=894.193767, last_y=834.987637,last_a=90.000000\n",
      "x=894.193767 y=846.547637 True_angle=6.199415 Des_Ro_dis=53.766787 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=83.800585 Robot_a=90\n",
      "0.974618747659\n",
      "25972\n",
      "False_epoche=3\n",
      "**********************************\n",
      "last_x=909.914953, last_y=844.904572,last_a=90.000000\n",
      "x=909.914953 y=856.464572 True_angle=12.829965 Des_Ro_dis=44.650193 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-77.170035 Robot_a=90\n",
      "0.955780907251\n",
      "45694\n",
      "False_epoche=4\n",
      "**********************************\n",
      "last_x=847.269715, last_y=882.975885,last_a=45.000000\n",
      "x=855.443870 y=891.150039 True_angle=33.765839 Des_Ro_dis=45.426540 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=11.234161 Robot_a=45\n",
      "0.953835547798\n",
      "47753\n",
      "False_epoche=5\n",
      "**********************************\n",
      "last_x=848.858417, last_y=863.487173,last_a=45.000000\n",
      "x=857.032571 y=871.661327 True_angle=11.593612 Des_Ro_dis=51.471160 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=33.406388 Robot_a=45\n",
      "0.952142882611\n",
      "49548\n",
      "False_epoche=6\n",
      "**********************************\n",
      "last_x=846.944107, last_y=891.120704,last_a=90.000000\n",
      "x=846.944107 y=902.680704 True_angle=92.892469 Des_Ro_dis=53.123572 Action=0 S_N=12\n",
      "L60D90/120\n",
      "Des_Robot_a=-2.892469 Robot_a=90\n",
      "0.951373466226\n",
      "50365\n",
      "False_epoche=7\n",
      "**********************************\n",
      "last_x=858.477662, last_y=858.144387,last_a=45.000000\n",
      "x=866.651816 y=866.318541 True_angle=0.284875 Des_Ro_dis=47.397701 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=45.284875 Robot_a=45\n",
      "0.941632983992\n",
      "60766\n",
      "False_epoche=8\n",
      "**********************************\n",
      "last_x=852.038504, last_y=872.046200,last_a=90.000000\n",
      "x=852.038504 y=883.606200 True_angle=71.128972 Des_Ro_dis=50.685913 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=18.871028 Robot_a=90\n",
      "0.941256680285\n",
      "61170\n",
      "False_epoche=9\n",
      "**********************************\n",
      "last_x=841.652518, last_y=878.985946,last_a=0.000000\n",
      "x=853.212518 y=878.985946 True_angle=24.186679 Des_Ro_dis=51.289950 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=24.186679 Robot_a=0\n",
      "0.940171467798\n",
      "62336\n",
      "False_epoche=10\n",
      "**********************************\n",
      "last_x=845.792628, last_y=895.300993,last_a=0.000000\n",
      "x=857.352628 y=895.300993 True_angle=6.287647 Des_Ro_dis=42.905466 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=6.287647 Robot_a=0\n",
      "0.939842245374\n",
      "62690\n",
      "False_epoche=11\n",
      "**********************************\n",
      "last_x=871.625410, last_y=842.375589,last_a=90.000000\n",
      "x=871.625410 y=853.935589 True_angle=31.632089 Des_Ro_dis=54.102193 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=58.367911 Robot_a=90\n",
      "0.939396957593\n",
      "63169\n",
      "False_epoche=12\n",
      "**********************************\n",
      "last_x=841.840485, last_y=886.644503,last_a=45.000000\n",
      "x=850.014639 y=894.818658 True_angle=39.082016 Des_Ro_dis=50.253185 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=5.917984 Robot_a=45\n",
      "0.936582367105\n",
      "66202\n",
      "False_epoche=13\n",
      "**********************************\n",
      "last_x=838.828045, last_y=906.026221,last_a=0.000000\n",
      "x=850.388045 y=906.026221 True_angle=6.925626 Des_Ro_dis=49.976608 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-6.925626 Robot_a=0\n",
      "0.934939142197\n",
      "67977\n",
      "False_epoche=14\n",
      "**********************************\n",
      "last_x=847.022062, last_y=882.396706,last_a=45.000000\n",
      "x=855.196217 y=890.570861 True_angle=33.115305 Des_Ro_dis=45.785234 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=11.884695 Robot_a=45\n",
      "0.932467313834\n",
      "70653\n",
      "False_epoche=15\n",
      "**********************************\n",
      "last_x=871.780918, last_y=842.227601,last_a=90.000000\n",
      "x=871.780918 y=853.787601 True_angle=31.409857 Des_Ro_dis=54.147045 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=58.590143 Robot_a=90\n",
      "0.92832829189\n",
      "75150\n",
      "False_epoche=16\n",
      "**********************************\n",
      "last_x=877.336413, last_y=843.464547,last_a=45.000000\n",
      "x=885.510567 y=851.638702 True_angle=28.321363 Des_Ro_dis=50.485234 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=73.321363 Robot_a=45\n",
      "0.919058040549\n",
      "85296\n",
      "False_epoche=17\n",
      "**********************************\n",
      "last_x=841.057549, last_y=881.133019,last_a=0.000000\n",
      "x=852.617549 y=881.133019 True_angle=21.711705 Des_Ro_dis=51.000585 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=21.711705 Robot_a=0\n",
      "0.918424647858\n",
      "85993\n",
      "False_epoche=18\n",
      "**********************************\n",
      "last_x=847.236408, last_y=870.137569,last_a=0.000000\n",
      "x=858.796408 y=870.137569 True_angle=35.932838 Des_Ro_dis=50.887138 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=35.932838 Robot_a=0\n",
      "0.918009592641\n",
      "86450\n",
      "False_epoche=19\n",
      "**********************************\n",
      "last_x=876.088952, last_y=849.858471,last_a=45.000000\n",
      "x=884.263107 y=858.032626 True_angle=24.444999 Des_Ro_dis=44.820869 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=69.444999 Robot_a=45\n",
      "0.916269699334\n",
      "88368\n",
      "False_epoche=20\n",
      "**********************************\n",
      "last_x=903.220495, last_y=841.550507,last_a=90.000000\n",
      "x=903.220495 y=853.110507 True_angle=3.929056 Des_Ro_dis=46.999959 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-86.070944 Robot_a=90\n",
      "0.915554934553\n",
      "89157\n",
      "False_epoche=21\n",
      "**********************************\n",
      "last_x=896.489903, last_y=844.888897,last_a=90.000000\n",
      "x=896.489903 y=856.448897 True_angle=4.607919 Des_Ro_dis=43.692326 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=85.392081 Robot_a=90\n",
      "0.914256395147\n",
      "90592\n",
      "False_epoche=22\n",
      "**********************************\n",
      "last_x=889.287239, last_y=846.631205,last_a=0.000000\n",
      "x=900.847239 y=846.631205 True_angle=90.909504 Des_Ro_dis=53.375519 Action=0 S_N=32\n",
      "L60D240/270\n",
      "Des_Robot_a=-89.090496 Robot_a=0\n",
      "0.913069885614\n",
      "91905\n",
      "False_epoche=23\n",
      "**********************************\n",
      "last_x=846.940437, last_y=885.954920,last_a=90.000000\n",
      "x=846.940437 y=897.514920 True_angle=87.318474 Des_Ro_dis=53.117726 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=2.681526 Robot_a=90\n",
      "0.911998577607\n",
      "93092\n",
      "False_epoche=24\n",
      "**********************************\n",
      "last_x=878.628172, last_y=842.241668,last_a=45.000000\n",
      "x=886.802326 y=850.415822 True_angle=30.095312 Des_Ro_dis=51.310518 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=75.095312 Robot_a=45\n",
      "0.910915927576\n",
      "94293\n",
      "False_epoche=25\n",
      "**********************************\n",
      "last_x=890.484792, last_y=838.456856,last_a=90.000000\n",
      "x=890.484792 y=850.016856 True_angle=10.778334 Des_Ro_dis=50.880781 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=79.221666 Robot_a=90\n",
      "0.907729630304\n",
      "97836\n",
      "False_epoche=26\n",
      "**********************************\n",
      "last_x=894.097423, last_y=842.920566,last_a=45.000000\n",
      "x=902.271578 y=851.094720 True_angle=47.659392 Des_Ro_dis=48.958007 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=-87.340608 Robot_a=45\n",
      "0.902006967037\n",
      "104231\n",
      "False_epoche=27\n",
      "**********************************\n",
      "last_x=854.084849, last_y=860.917690,last_a=45.000000\n",
      "x=862.259003 y=869.091844 True_angle=5.684087 Des_Ro_dis=48.782138 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=39.315913 Robot_a=45\n",
      "0.901201848311\n",
      "105134\n",
      "False_epoche=28\n",
      "**********************************\n",
      "last_x=847.638926, last_y=877.995958,last_a=45.000000\n",
      "x=855.813081 y=886.170112 True_angle=27.620609 Des_Ro_dis=46.300644 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=17.379391 Robot_a=45\n",
      "0.900368964017\n",
      "106069\n",
      "False_epoche=29\n",
      "**********************************\n",
      "last_x=844.130276, last_y=894.780354,last_a=45.000000\n",
      "x=852.304430 y=902.954508 True_angle=48.544665 Des_Ro_dis=47.786991 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=-3.544665 Robot_a=45\n",
      "0.897679667513\n",
      "109094\n",
      "False_epoche=30\n",
      "**********************************\n",
      "last_x=853.620445, last_y=860.493173,last_a=45.000000\n",
      "x=861.794600 y=868.667328 True_angle=5.644442 Des_Ro_dis=49.410414 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=39.355558 Robot_a=45\n",
      "0.89672149098\n",
      "110174\n",
      "False_epoche=31\n",
      "**********************************\n",
      "last_x=848.472421, last_y=871.003886,last_a=45.000000\n",
      "x=856.646575 y=879.178040 True_angle=19.345767 Des_Ro_dis=48.094422 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=25.654233 Robot_a=45\n",
      "0.895252524858\n",
      "111832\n",
      "False_epoche=32\n",
      "**********************************\n",
      "last_x=890.802244, last_y=837.494929,last_a=90.000000\n",
      "x=890.802244 y=849.054929 True_angle=10.234085 Des_Ro_dis=51.768706 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=79.765915 Robot_a=90\n",
      "0.894189090575\n",
      "113034\n",
      "False_epoche=33\n",
      "**********************************\n",
      "last_x=848.954374, last_y=865.087543,last_a=45.000000\n",
      "x=857.128528 y=873.261697 True_angle=13.048820 Des_Ro_dis=50.526230 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=31.951180 Robot_a=45\n",
      "0.893840788693\n",
      "113428\n",
      "False_epoche=34\n",
      "**********************************\n",
      "last_x=843.708782, last_y=899.890067,last_a=45.000000\n",
      "x=851.882937 y=908.064221 True_angle=54.514115 Des_Ro_dis=48.788149 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=-9.514115 Robot_a=45\n",
      "0.89354298453\n",
      "113765\n",
      "False_epoche=35\n",
      "**********************************\n",
      "last_x=857.232921, last_y=865.130302,last_a=90.000000\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "x=857.232921 y=876.690302 True_angle=61.407922 Des_Ro_dis=48.706931 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=28.592078 Robot_a=90\n",
      "0.892734912449\n",
      "114680\n",
      "False_epoche=36\n",
      "**********************************\n",
      "last_x=842.739117, last_y=878.231876,last_a=0.000000\n",
      "x=854.299117 y=878.231876 True_angle=25.469271 Des_Ro_dis=50.620370 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=25.469271 Robot_a=0\n",
      "0.890249986879\n",
      "117499\n",
      "False_epoche=37\n",
      "**********************************\n",
      "last_x=893.360998, last_y=842.662409,last_a=90.000000\n",
      "x=893.360998 y=854.222409 True_angle=8.251922 Des_Ro_dis=46.256504 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=81.748078 Robot_a=90\n",
      "0.888134651179\n",
      "119905\n",
      "False_epoche=38\n",
      "**********************************\n",
      "last_x=851.743194, last_y=873.172076,last_a=0.000000\n",
      "x=863.303194 y=873.172076 True_angle=36.169408 Des_Ro_dis=45.457596 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=36.169408 Robot_a=0\n",
      "0.887485071911\n",
      "120645\n",
      "False_epoche=39\n",
      "**********************************\n",
      "last_x=891.736334, last_y=840.731036,last_a=90.000000\n",
      "x=891.736334 y=852.291036 True_angle=9.826699 Des_Ro_dis=48.419350 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=80.173301 Robot_a=90\n",
      "0.887120114038\n",
      "121061\n",
      "False_epoche=40\n",
      "**********************************\n",
      "last_x=876.199385, last_y=841.331448,last_a=90.000000\n",
      "x=876.199385 y=852.891448 True_angle=26.804237 Des_Ro_dis=52.779588 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=63.195763 Robot_a=90\n",
      "0.886843864714\n",
      "121376\n",
      "False_epoche=41\n",
      "**********************************\n",
      "last_x=869.018906, last_y=853.598970,last_a=45.000000\n",
      "x=877.193060 y=861.773125 True_angle=14.178829 Des_Ro_dis=44.513487 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=59.178829 Robot_a=45\n",
      "0.884812819698\n",
      "123695\n",
      "False_epoche=42\n",
      "**********************************\n",
      "last_x=884.677004, last_y=848.393072,last_a=45.000000\n",
      "x=892.851159 y=856.567227 True_angle=35.653175 Des_Ro_dis=44.017175 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=80.653175 Robot_a=45\n",
      "0.883623882924\n",
      "125055\n",
      "False_epoche=43\n",
      "**********************************\n",
      "last_x=867.842375, last_y=844.765866,last_a=45.000000\n",
      "x=876.016529 y=852.940020 True_angle=17.994961 Des_Ro_dis=52.819017 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=62.994961 Robot_a=45\n",
      "0.882680005764\n",
      "126136\n",
      "False_epoche=44\n",
      "**********************************\n",
      "last_x=886.349653, last_y=847.896947,last_a=0.000000\n",
      "x=897.909653 y=847.896947 True_angle=87.702556 Des_Ro_dis=52.144968 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=87.702556 Robot_a=0\n",
      "0.88219405835\n",
      "126693\n",
      "False_epoche=45\n",
      "**********************************\n",
      "last_x=867.786105, last_y=849.621952,last_a=90.000000\n",
      "x=867.786105 y=861.181952 True_angle=39.688229 Des_Ro_dis=50.443790 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=50.311771 Robot_a=90\n",
      "0.881487867208\n",
      "127503\n",
      "False_epoche=46\n",
      "**********************************\n",
      "last_x=840.604608, last_y=897.295530,last_a=315.000000\n",
      "x=848.778763 y=889.121375 True_angle=56.990597 Des_Ro_dis=52.363724 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=11.990597 Robot_a=315\n",
      "0.880612461867\n",
      "128508\n",
      "False_epoche=47\n",
      "**********************************\n",
      "last_x=834.933490, last_y=901.470708,last_a=0.000000\n",
      "x=846.493490 y=901.470708 True_angle=1.574466 Des_Ro_dis=53.526719 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-1.574466 Robot_a=0\n",
      "0.878152111637\n",
      "131338\n",
      "False_epoche=48\n",
      "**********************************\n",
      "last_x=910.374404, last_y=845.139887,last_a=90.000000\n",
      "x=910.374404 y=856.699887 True_angle=13.473682 Des_Ro_dis=44.525589 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-76.526318 Robot_a=90\n",
      "0.877762398836\n",
      "131787\n",
      "False_epoche=49\n",
      "**********************************\n",
      "last_x=839.557706, last_y=898.151728,last_a=0.000000\n",
      "x=851.117706 y=898.151728 True_angle=2.165360 Des_Ro_dis=48.917224 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=2.165360 Robot_a=0\n",
      "0.877019912052\n",
      "132643\n",
      "False_epoche=50\n",
      "**********************************\n",
      "last_x=844.353739, last_y=892.746796,last_a=0.000000\n",
      "x=855.913739 y=892.746796 True_angle=9.342778 Des_Ro_dis=44.678936 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=9.342778 Robot_a=0\n",
      "0.876256403885\n",
      "133524\n",
      "False_epoche=51\n",
      "**********************************\n",
      "last_x=874.296444, last_y=849.277079,last_a=90.000000\n",
      "x=874.296444 y=860.837079 True_angle=33.277843 Des_Ro_dis=46.844500 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=56.722157 Robot_a=90\n",
      "0.8754433709\n",
      "134463\n",
      "False_epoche=52\n",
      "**********************************\n",
      "last_x=838.285428, last_y=885.317142,last_a=0.000000\n",
      "x=849.845428 y=885.317142 True_angle=16.317505 Des_Ro_dis=52.259615 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=16.317505 Robot_a=0\n",
      "0.875170799171\n",
      "134778\n",
      "False_epoche=53\n",
      "**********************************\n",
      "last_x=864.784665, last_y=858.351214,last_a=45.000000\n",
      "x=872.958819 y=866.525368 True_angle=6.068336 Des_Ro_dis=43.032272 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=51.068336 Robot_a=45\n",
      "0.873652028707\n",
      "136535\n",
      "False_epoche=54\n",
      "**********************************\n",
      "last_x=866.286970, last_y=855.808578,last_a=45.000000\n",
      "x=874.461124 y=863.982732 True_angle=9.660572 Des_Ro_dis=44.152891 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=54.660572 Robot_a=45\n",
      "0.872545536219\n",
      "137817\n",
      "False_epoche=55\n",
      "**********************************\n",
      "last_x=855.241566, last_y=866.002976,last_a=0.000000\n",
      "x=866.801566 y=866.002976 True_angle=45.680905 Des_Ro_dis=47.517719 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=45.680905 Robot_a=0\n",
      "0.868761249348\n",
      "142214\n",
      "False_epoche=56\n",
      "**********************************\n",
      "last_x=890.178655, last_y=840.379111,last_a=90.000000\n",
      "x=890.178655 y=851.939111 True_angle=11.549498 Des_Ro_dis=49.054132 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=78.450502 Robot_a=90\n",
      "0.867457781245\n",
      "143733\n",
      "False_epoche=57\n",
      "**********************************\n",
      "last_x=875.589803, last_y=843.297896,last_a=90.000000\n",
      "x=875.589803 y=854.857896 True_angle=28.401918 Des_Ro_dis=51.319268 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=61.598082 Robot_a=90\n",
      "0.867054013725\n",
      "144204\n",
      "False_epoche=58\n",
      "**********************************\n",
      "last_x=840.444696, last_y=889.684581,last_a=45.000000\n",
      "x=848.618851 y=897.858735 True_angle=42.613629 Des_Ro_dis=51.425748 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=2.386371 Robot_a=45\n",
      "0.86656391901\n",
      "144776\n",
      "False_epoche=59\n",
      "**********************************\n",
      "last_x=845.199836, last_y=911.703256,last_a=315.000000\n",
      "x=853.373990 y=903.529101 True_angle=40.671562 Des_Ro_dis=46.759377 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=-4.328438 Robot_a=315\n",
      "0.864461070356\n",
      "147234\n",
      "False_epoche=60\n",
      "**********************************\n",
      "last_x=843.667112, last_y=879.379223,last_a=45.000000\n",
      "x=851.841267 y=887.553377 True_angle=30.509002 Des_Ro_dis=49.741150 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=14.490998 Robot_a=45\n",
      "0.864088606533\n",
      "147670\n",
      "False_epoche=61\n",
      "**********************************\n",
      "last_x=901.348252, last_y=837.104269,last_a=90.000000\n",
      "x=901.348252 y=848.664269 True_angle=1.504437 Des_Ro_dis=51.353433 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-88.495563 Robot_a=90\n",
      "0.86408091977\n",
      "147679\n",
      "False_epoche=62\n",
      "**********************************\n",
      "last_x=865.999633, last_y=853.456471,last_a=0.000000\n",
      "x=877.559633 y=853.456471 True_angle=64.259555 Des_Ro_dis=51.670786 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=64.259555 Robot_a=0\n",
      "0.863583989295\n",
      "148261\n",
      "False_epoche=63\n",
      "**********************************\n",
      "last_x=852.233140, last_y=870.493741,last_a=0.000000\n",
      "x=863.793140 y=870.493741 True_angle=39.177817 Des_Ro_dis=46.707130 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=39.177817 Robot_a=0\n",
      "0.862615721064\n",
      "149396\n",
      "False_epoche=64\n",
      "**********************************\n",
      "last_x=834.573280, last_y=906.143967,last_a=0.000000\n",
      "x=846.133280 y=906.143967 True_angle=6.506961 Des_Ro_dis=54.215974 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-6.506961 Robot_a=0\n",
      "0.861947532303\n",
      "150180\n",
      "False_epoche=65\n",
      "**********************************\n",
      "last_x=862.524531, last_y=850.695831,last_a=45.000000\n",
      "x=870.698685 y=858.869985 True_angle=9.533658 Des_Ro_dis=50.499952 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=54.533658 Robot_a=45\n",
      "0.860707999295\n",
      "151636\n",
      "False_epoche=66\n",
      "**********************************\n",
      "last_x=903.785712, last_y=835.254667,last_a=90.000000\n",
      "x=903.785712 y=846.814667 True_angle=4.071425 Des_Ro_dis=53.319895 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-85.928575 Robot_a=90\n",
      "0.859483011883\n",
      "153077\n",
      "False_epoche=67\n",
      "**********************************\n",
      "last_x=867.051567, last_y=851.866640,last_a=0.000000\n",
      "x=878.611567 y=851.866640 True_angle=66.041658 Des_Ro_dis=52.671486 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=66.041658 Robot_a=0\n",
      "0.858577945478\n",
      "154143\n",
      "False_epoche=68\n",
      "**********************************\n",
      "last_x=864.941364, last_y=849.663763,last_a=45.000000\n",
      "x=873.115519 y=857.837917 True_angle=12.476544 Des_Ro_dis=50.004165 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=57.476544 Robot_a=45\n",
      "0.857865439299\n",
      "154983\n",
      "False_epoche=69\n",
      "**********************************\n",
      "last_x=841.582674, last_y=891.018210,last_a=45.000000\n",
      "x=849.756829 y=899.192364 True_angle=44.079076 Des_Ro_dis=50.249662 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=0.920924 Robot_a=45\n",
      "0.8571348942\n",
      "155845\n",
      "False_epoche=70\n",
      "**********************************\n",
      "last_x=877.423347, last_y=848.079113,last_a=45.000000\n",
      "x=885.597502 y=856.253267 True_angle=26.777186 Des_Ro_dis=46.056580 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=71.777186 Robot_a=45\n",
      "0.856952779776\n",
      "156060\n",
      "False_epoche=71\n",
      "**********************************\n",
      "last_x=876.150070, last_y=849.133679,last_a=0.000000\n",
      "x=887.710070 y=849.133679 True_angle=76.416951 Des_Ro_dis=52.329962 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=76.416951 Robot_a=0\n",
      "0.855056038685\n",
      "158302\n",
      "False_epoche=72\n",
      "**********************************\n",
      "last_x=901.121427, last_y=837.953591,last_a=90.000000\n",
      "x=901.121427 y=849.513591 True_angle=1.272471 Des_Ro_dis=50.498862 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-88.727529 Robot_a=90\n",
      "0.85388475843\n",
      "159689\n",
      "False_epoche=73\n",
      "**********************************\n",
      "last_x=836.616378, last_y=894.360632,last_a=0.000000\n",
      "x=848.176378 y=894.360632 True_angle=6.210403 Des_Ro_dis=52.129553 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=6.210403 Robot_a=0\n",
      "0.853678031989\n",
      "159934\n",
      "False_epoche=74\n",
      "**********************************\n",
      "last_x=855.458447, last_y=861.630811,last_a=45.000000\n",
      "x=863.632602 y=869.804966 True_angle=5.297912 Des_Ro_dis=47.268676 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=39.702088 Robot_a=45\n",
      "0.853334724898\n",
      "160341\n",
      "False_epoche=75\n",
      "**********************************\n",
      "last_x=834.866332, last_y=903.948190,last_a=0.000000\n",
      "x=846.426332 y=903.948190 True_angle=4.214877 Des_Ro_dis=53.718955 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-4.214877 Robot_a=0\n",
      "0.852946037189\n",
      "160802\n",
      "False_epoche=76\n",
      "**********************************\n",
      "last_x=856.580489, last_y=865.889804,last_a=45.000000\n",
      "x=864.754644 y=874.063958 True_angle=8.651719 Des_Ro_dis=43.759724 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=36.348281 Robot_a=45\n",
      "0.852418518101\n",
      "161428\n",
      "False_epoche=77\n",
      "**********************************\n",
      "last_x=839.547486, last_y=882.495520,last_a=0.000000\n",
      "x=851.107486 y=882.495520 True_angle=19.698364 Des_Ro_dis=51.931539 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=19.698364 Robot_a=0\n",
      "0.852071513159\n",
      "161840\n",
      "False_epoche=78\n",
      "**********************************\n",
      "last_x=883.398612, last_y=841.337503,last_a=90.000000\n",
      "x=883.398612 y=852.897503 True_angle=19.415128 Des_Ro_dis=49.942480 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=70.584872 Robot_a=90\n",
      "0.851255099816\n",
      "162810\n",
      "False_epoche=79\n",
      "**********************************\n",
      "last_x=854.319847, last_y=868.276468,last_a=45.000000\n",
      "x=862.494002 y=876.450623 True_angle=12.875988 Des_Ro_dis=44.286263 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=32.124012 Robot_a=45\n",
      "0.850581524284\n",
      "163611\n",
      "False_epoche=80\n",
      "**********************************\n",
      "last_x=873.155735, last_y=843.349622,last_a=90.000000\n",
      "x=873.155735 y=854.909622 True_angle=30.767180 Des_Ro_dis=52.476249 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=59.232820 Robot_a=90\n",
      "0.849771594143\n",
      "164575\n",
      "False_epoche=81\n",
      "**********************************\n",
      "last_x=842.137361, last_y=879.966785,last_a=45.000000\n",
      "x=850.311516 y=888.140939 True_angle=31.576440 Des_Ro_dis=51.084076 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=13.423560 Robot_a=45\n",
      "0.849283828557\n",
      "165156\n",
      "False_epoche=82\n",
      "**********************************\n",
      "last_x=841.748744, last_y=870.418731,last_a=45.000000\n",
      "x=849.922899 y=878.592886 True_angle=21.854111 Des_Ro_dis=54.460817 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=23.145889 Robot_a=45\n",
      "0.847867474618\n",
      "166845\n",
      "False_epoche=83\n",
      "**********************************\n",
      "last_x=851.323574, last_y=872.412794,last_a=45.000000\n",
      "x=859.497729 y=880.586949 True_angle=19.391240 Des_Ro_dis=44.914369 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=25.608760 Robot_a=45\n",
      "0.847241821325\n",
      "167592\n",
      "False_epoche=84\n",
      "**********************************\n",
      "last_x=883.769158, last_y=838.412047,last_a=90.000000\n",
      "x=883.769158 y=849.972047 True_angle=17.974902 Des_Ro_dis=52.595022 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=72.025098 Robot_a=90\n",
      "0.846887742928\n",
      "168015\n",
      "False_epoche=85\n",
      "**********************************\n",
      "last_x=847.676224, last_y=878.889395,last_a=0.000000\n",
      "x=859.236224 y=878.889395 True_angle=27.378581 Des_Ro_dis=45.905806 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=27.378581 Robot_a=0\n",
      "0.846115651916\n",
      "168938\n",
      "False_epoche=86\n",
      "**********************************\n",
      "last_x=842.464749, last_y=898.742116,last_a=315.000000\n",
      "x=850.638903 y=890.567961 True_angle=55.817813 Des_Ro_dis=50.254166 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=10.817813 Robot_a=315\n",
      "0.844974297658\n",
      "170304\n",
      "False_epoche=87\n",
      "**********************************\n",
      "last_x=852.434622, last_y=870.862851,last_a=45.000000\n",
      "x=860.608777 y=879.037006 True_angle=16.979242 Des_Ro_dis=44.621919 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=28.020758 Robot_a=45\n",
      "0.844346633017\n",
      "171056\n",
      "False_epoche=88\n",
      "**********************************\n",
      "last_x=847.348045, last_y=888.047091,last_a=45.000000\n",
      "x=855.522199 y=896.221246 True_angle=40.143915 Des_Ro_dis=44.638030 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=4.856085 Robot_a=45\n",
      "0.8436677512\n",
      "171870\n",
      "False_epoche=89\n",
      "**********************************\n",
      "last_x=854.894542, last_y=865.916368,last_a=0.000000\n",
      "x=866.454542 y=865.916368 True_angle=45.455935 Des_Ro_dis=47.822502 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=45.455935 Robot_a=0\n",
      "0.843387685889\n",
      "172206\n",
      "False_epoche=90\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "**********************************\n",
      "last_x=843.177450, last_y=891.618997,last_a=315.000000\n",
      "x=851.351604 y=883.444843 True_angle=63.793553 Des_Ro_dis=51.388128 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=18.793553 Robot_a=315\n",
      "0.842493940483\n",
      "173279\n",
      "False_epoche=91\n",
      "**********************************\n",
      "last_x=875.277240, last_y=845.624776,last_a=45.000000\n",
      "x=883.451394 y=853.798931 True_angle=25.293155 Des_Ro_dis=49.075403 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=70.293155 Robot_a=45\n",
      "0.840015971294\n",
      "176260\n",
      "False_epoche=92\n",
      "**********************************\n",
      "last_x=871.204130, last_y=842.241898,last_a=45.000000\n",
      "x=879.378285 y=850.416053 True_angle=22.417819 Des_Ro_dis=53.701238 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=67.417819 Robot_a=45\n",
      "0.839273439333\n",
      "177155\n",
      "False_epoche=93\n",
      "**********************************\n",
      "last_x=874.604383, last_y=848.917502,last_a=90.000000\n",
      "x=874.604383 y=860.477502 True_angle=32.723220 Des_Ro_dis=46.978348 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=57.276780 Robot_a=90\n",
      "0.8388415004\n",
      "177676\n",
      "False_epoche=94\n",
      "**********************************\n",
      "last_x=865.418067, last_y=855.527939,last_a=45.000000\n",
      "x=873.592221 y=863.702093 True_angle=8.963025 Des_Ro_dis=44.887736 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=53.963025 Robot_a=45\n",
      "0.838341859633\n",
      "178279\n",
      "False_epoche=95\n",
      "**********************************\n",
      "last_x=836.217140, last_y=904.020261,last_a=0.000000\n",
      "x=847.777140 y=904.020261 True_angle=4.402106 Des_Ro_dis=52.377377 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-4.402106 Robot_a=0\n",
      "0.838036257873\n",
      "178648\n",
      "False_epoche=96\n",
      "**********************************\n",
      "last_x=865.959720, last_y=849.204106,last_a=90.000000\n",
      "x=865.959720 y=860.764106 True_angle=40.944257 Des_Ro_dis=51.944163 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=49.055743 Robot_a=90\n",
      "0.837365820068\n",
      "179458\n",
      "False_epoche=97\n",
      "**********************************\n",
      "last_x=854.626869, last_y=868.804161,last_a=45.000000\n",
      "x=862.801024 y=876.978316 True_angle=13.247484 Des_Ro_dis=43.746563 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=31.752516 Robot_a=45\n",
      "0.836768677541\n",
      "180180\n",
      "False_epoche=98\n",
      "**********************************\n",
      "last_x=859.744522, last_y=856.473852,last_a=0.000000\n",
      "x=871.304522 y=856.473852 True_angle=56.604339 Des_Ro_dis=52.134020 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=56.604339 Robot_a=0\n",
      "0.836264502436\n",
      "180790\n",
      "False_epoche=99\n",
      "**********************************\n",
      "last_x=850.913260, last_y=868.645394,last_a=90.000000\n",
      "x=850.913260 y=880.205394 True_angle=68.037864 Des_Ro_dis=52.927635 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=21.962136 Robot_a=90\n",
      "0.835486527759\n",
      "181732\n",
      "False_epoche=100\n",
      "**********************************\n",
      "last_x=884.088591, last_y=844.521150,last_a=45.000000\n",
      "x=892.262745 y=852.695304 True_angle=35.710837 Des_Ro_dis=47.933281 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=80.710837 Robot_a=45\n",
      "0.835049135818\n",
      "182262\n",
      "False_epoche=101\n",
      "**********************************\n",
      "last_x=838.733815, last_y=885.175416,last_a=0.000000\n",
      "x=850.293815 y=885.175416 True_angle=16.606909 Des_Ro_dis=51.869771 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=16.606909 Robot_a=0\n",
      "0.83473155304\n",
      "182647\n",
      "False_epoche=102\n",
      "**********************************\n",
      "last_x=844.640554, last_y=875.992794,last_a=0.000000\n",
      "x=856.200554 y=875.992794 True_angle=28.727881 Des_Ro_dis=49.947346 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=28.727881 Robot_a=0\n",
      "0.834449543075\n",
      "182989\n",
      "False_epoche=103\n",
      "**********************************\n",
      "last_x=849.701270, last_y=870.118150,last_a=45.000000\n",
      "x=857.875424 y=878.292304 True_angle=17.736980 Des_Ro_dis=47.388859 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=27.263020 Robot_a=45\n",
      "0.834249226175\n",
      "183232\n",
      "False_epoche=104\n",
      "**********************************\n",
      "last_x=845.886478, last_y=881.732708,last_a=45.000000\n",
      "x=854.060632 y=889.906862 True_angle=32.608670 Des_Ro_dis=47.035061 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=12.391330 Robot_a=45\n",
      "0.833585972544\n",
      "184037\n",
      "False_epoche=105\n",
      "**********************************\n",
      "last_x=849.293914, last_y=884.235124,last_a=45.000000\n",
      "x=857.468068 y=892.409278 True_angle=34.880898 Des_Ro_dis=43.203985 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=10.119102 Robot_a=45\n",
      "0.83334716724\n",
      "184327\n",
      "False_epoche=106\n",
      "**********************************\n",
      "last_x=850.281436, last_y=859.173792,last_a=45.000000\n",
      "x=858.455590 y=867.347947 True_angle=6.834233 Des_Ro_dis=52.840274 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=38.165767 Robot_a=45\n",
      "0.833156172853\n",
      "184559\n",
      "False_epoche=107\n",
      "**********************************\n",
      "last_x=881.685700, last_y=839.923045,last_a=45.000000\n",
      "x=889.859854 y=848.097200 True_angle=33.945481 Des_Ro_dis=52.884055 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=78.945481 Robot_a=45\n",
      "0.832418955245\n",
      "185455\n",
      "False_epoche=108\n",
      "**********************************\n",
      "last_x=902.431027, last_y=835.035464,last_a=90.000000\n",
      "x=902.431027 y=846.595464 True_angle=2.606361 Des_Ro_dis=53.459839 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-87.393639 Robot_a=90\n",
      "0.831204317868\n",
      "186933\n",
      "False_epoche=109\n",
      "**********************************\n",
      "last_x=876.520706, last_y=847.985193,last_a=90.000000\n",
      "x=876.520706 y=859.545193 True_angle=30.130164 Des_Ro_dis=46.774658 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=59.869836 Robot_a=90\n",
      "0.830868513978\n",
      "187342\n",
      "False_epoche=110\n",
      "**********************************\n",
      "last_x=863.951261, last_y=860.550675,last_a=0.000000\n",
      "x=875.511261 y=860.550675 True_angle=58.169460 Des_Ro_dis=46.432182 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=58.169460 Robot_a=0\n",
      "0.830203879695\n",
      "188152\n",
      "False_epoche=111\n",
      "**********************************\n",
      "last_x=866.354766, last_y=854.620447,last_a=0.000000\n",
      "x=877.914766 y=854.620447 True_angle=64.048828 Des_Ro_dis=50.468420 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=64.048828 Robot_a=0\n",
      "0.82992341793\n",
      "188494\n",
      "False_epoche=112\n",
      "**********************************\n",
      "last_x=869.983004, last_y=844.353778,last_a=90.000000\n",
      "x=869.983004 y=855.913778 True_angle=34.249783 Des_Ro_dis=53.334932 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=55.750217 Robot_a=90\n",
      "0.829288223451\n",
      "189269\n",
      "False_epoche=113\n",
      "**********************************\n",
      "last_x=879.211535, last_y=844.243222,last_a=45.000000\n",
      "x=887.385689 y=852.417377 True_angle=30.152242 Des_Ro_dis=49.226283 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=75.152242 Robot_a=45\n",
      "0.829226779138\n",
      "189344\n",
      "False_epoche=114\n",
      "**********************************\n",
      "last_x=838.244078, last_y=905.159560,last_a=0.000000\n",
      "x=849.804078 y=905.159560 True_angle=5.868733 Des_Ro_dis=50.460397 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-5.868733 Robot_a=0\n",
      "0.828639604065\n",
      "190061\n",
      "False_epoche=115\n",
      "**********************************\n",
      "last_x=859.377056, last_y=859.859460,last_a=45.000000\n",
      "x=867.551210 y=868.033614 True_angle=0.429079 Des_Ro_dis=45.549685 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=44.570921 Robot_a=45\n",
      "0.827658643383\n",
      "191260\n",
      "False_epoche=116\n",
      "**********************************\n",
      "last_x=844.435189, last_y=878.778178,last_a=45.000000\n",
      "x=852.609343 y=886.952332 True_angle=29.606623 Des_Ro_dis=49.154003 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=15.393377 Robot_a=45\n",
      "0.825674960696\n",
      "193689\n",
      "False_epoche=117\n",
      "**********************************\n",
      "last_x=899.837177, last_y=843.040995,last_a=90.000000\n",
      "x=899.837177 y=854.600995 True_angle=0.205490 Des_Ro_dis=45.399297 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=89.794510 Robot_a=90\n",
      "0.825125381002\n",
      "194363\n",
      "False_epoche=118\n",
      "**********************************\n",
      "last_x=845.694657, last_y=864.307252,last_a=45.000000\n",
      "x=853.868812 y=872.481406 True_angle=14.182696 Des_Ro_dis=53.715543 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=30.817304 Robot_a=45\n",
      "0.824808358894\n",
      "194752\n",
      "False_epoche=119\n",
      "**********************************\n",
      "last_x=843.240008, last_y=869.213074,last_a=0.000000\n",
      "x=854.800008 y=869.213074 True_angle=34.259826 Des_Ro_dis=54.688884 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=34.259826 Robot_a=0\n",
      "0.824045241038\n",
      "195689\n",
      "False_epoche=120\n",
      "**********************************\n",
      "last_x=845.994342, last_y=873.506298,last_a=45.000000\n",
      "x=854.168496 y=881.680453 True_angle=23.212660 Des_Ro_dis=49.357193 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=21.787340 Robot_a=45\n",
      "0.82348537053\n",
      "196377\n",
      "False_epoche=121\n",
      "**********************************\n",
      "last_x=864.379521, last_y=862.380382,last_a=0.000000\n",
      "x=875.939521 y=862.380382 True_angle=57.398130 Des_Ro_dis=44.655821 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=57.398130 Robot_a=0\n",
      "0.823396705456\n",
      "196486\n",
      "False_epoche=122\n",
      "**********************************\n",
      "last_x=846.366742, last_y=913.488345,last_a=0.000000\n",
      "x=857.926742 y=913.488345 True_angle=17.775429 Des_Ro_dis=44.182513 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-17.775429 Robot_a=0\n",
      "0.823155975661\n",
      "196782\n",
      "False_epoche=123\n",
      "**********************************\n",
      "last_x=844.734577, last_y=883.329536,last_a=0.000000\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "x=856.294577 y=883.329536 True_angle=20.878243 Des_Ro_dis=46.776793 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=20.878243 Robot_a=0\n",
      "0.823025068088\n",
      "196943\n",
      "False_epoche=124\n",
      "**********************************\n",
      "last_x=852.916698, last_y=859.952098,last_a=90.000000\n",
      "x=852.916698 y=871.512098 True_angle=58.823831 Des_Ro_dis=55.030882 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=31.176169 Robot_a=90\n",
      "0.822664165084\n",
      "197387\n",
      "False_epoche=125\n",
      "**********************************\n",
      "last_x=848.642581, last_y=887.957518,last_a=45.000000\n",
      "x=856.816735 y=896.131673 True_angle=39.881146 Des_Ro_dis=43.356180 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=5.118854 Robot_a=45\n",
      "0.82191604595\n",
      "198308\n",
      "False_epoche=126\n",
      "**********************************\n",
      "last_x=857.891802, last_y=860.603123,last_a=90.000000\n",
      "x=857.891802 y=872.163123 True_angle=56.532117 Des_Ro_dis=50.477639 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=33.467883 Robot_a=90\n",
      "0.821675754365\n",
      "198604\n",
      "False_epoche=127\n",
      "**********************************\n",
      "last_x=864.071451, last_y=857.848476,last_a=45.000000\n",
      "x=872.245606 y=866.022630 True_angle=5.756352 Des_Ro_dis=43.872179 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=50.756352 Robot_a=45\n",
      "0.821591344476\n",
      "198708\n",
      "False_epoche=128\n",
      "**********************************\n",
      "last_x=849.999475, last_y=868.593438,last_a=90.000000\n",
      "x=849.999475 y=880.153438 True_angle=68.350532 Des_Ro_dis=53.795339 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=21.649468 Robot_a=90\n",
      "0.82149720534\n",
      "198824\n",
      "False_epoche=129\n",
      "**********************************\n",
      "last_x=878.774692, last_y=847.524709,last_a=90.000000\n",
      "x=878.774692 y=859.084709 True_angle=27.418579 Des_Ro_dis=46.093109 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=62.581421 Robot_a=90\n",
      "0.821196195718\n",
      "199195\n",
      "False_epoche=130\n",
      "**********************************\n",
      "last_x=851.316757, last_y=865.318526,last_a=45.000000\n",
      "x=859.490911 y=873.492680 True_angle=11.801075 Des_Ro_dis=48.410993 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=33.198925 Robot_a=45\n",
      "0.820718541841\n",
      "199784\n",
      "False_epoche=131\n",
      "**********************************\n",
      "last_x=836.763016, last_y=910.164581,last_a=0.000000\n",
      "x=848.323016 y=910.164581 True_angle=11.127713 Des_Ro_dis=52.667156 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-11.127713 Robot_a=0\n",
      "0.819776224241\n",
      "200947\n",
      "False_epoche=132\n",
      "**********************************\n",
      "last_x=848.759713, last_y=879.661979,last_a=45.000000\n",
      "x=856.933867 y=887.836134 True_angle=29.227868 Des_Ro_dis=44.750994 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=15.772132 Robot_a=45\n",
      "0.818948246746\n",
      "201970\n",
      "False_epoche=133\n",
      "**********************************\n",
      "last_x=843.545139, last_y=881.069226,last_a=45.000000\n",
      "x=851.719294 y=889.243380 True_angle=32.440013 Des_Ro_dis=49.464447 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=12.559987 Robot_a=45\n",
      "0.818417750718\n",
      "202626\n",
      "False_epoche=134\n",
      "**********************************\n",
      "last_x=840.861178, last_y=881.044448,last_a=0.000000\n",
      "x=852.421178 y=881.044448 True_angle=21.722492 Des_Ro_dis=51.215791 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=21.722492 Robot_a=0\n",
      "0.817469225172\n",
      "203800\n",
      "False_epoche=135\n",
      "**********************************\n",
      "last_x=846.528503, last_y=876.311383,last_a=0.000000\n",
      "x=858.088503 y=876.311383 True_angle=29.475382 Des_Ro_dis=48.142748 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=29.475382 Robot_a=0\n",
      "0.817022818134\n",
      "204353\n",
      "False_epoche=136\n",
      "**********************************\n",
      "last_x=840.948234, last_y=910.391538,last_a=0.000000\n",
      "x=852.508234 y=910.391538 True_angle=12.342212 Des_Ro_dis=48.615347 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-12.342212 Robot_a=0\n",
      "0.816977626121\n",
      "204409\n",
      "False_epoche=137\n",
      "**********************************\n",
      "last_x=858.297539, last_y=857.406122,last_a=45.000000\n",
      "x=866.471694 y=865.580277 True_angle=0.751626 Des_Ro_dis=48.050647 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=45.751626 Robot_a=45\n",
      "0.816601662154\n",
      "204875\n",
      "False_epoche=138\n",
      "**********************************\n",
      "last_x=847.923702, last_y=881.572129,last_a=45.000000\n",
      "x=856.097857 y=889.746283 True_angle=31.853745 Des_Ro_dis=45.083666 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=13.146255 Robot_a=45\n",
      "0.816354879805\n",
      "205181\n",
      "False_epoche=139\n",
      "**********************************\n",
      "last_x=849.289464, last_y=883.554462,last_a=0.000000\n",
      "x=860.849464 y=883.554462 True_angle=22.785283 Des_Ro_dis=42.464340 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=22.785283 Robot_a=0\n",
      "0.816114621851\n",
      "205479\n",
      "False_epoche=140\n",
      "**********************************\n",
      "last_x=869.097299, last_y=851.545802,last_a=0.000000\n",
      "x=880.657299 y=851.545802 True_angle=68.238335 Des_Ro_dis=52.172305 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=68.238335 Robot_a=0\n",
      "0.816020311957\n",
      "205596\n",
      "False_epoche=141\n",
      "**********************************\n",
      "last_x=871.607998, last_y=850.861285,last_a=90.000000\n",
      "x=871.607998 y=862.421285 True_angle=37.072292 Des_Ro_dis=47.098467 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=52.927708 Robot_a=90\n",
      "0.815722139607\n",
      "205966\n",
      "False_epoche=142\n",
      "**********************************\n",
      "last_x=845.133909, last_y=887.073626,last_a=90.000000\n",
      "x=845.133909 y=898.633626 True_angle=88.573413 Des_Ro_dis=54.883103 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=1.426587 Robot_a=90\n",
      "0.814155728935\n",
      "207912\n",
      "False_epoche=143\n",
      "**********************************\n",
      "last_x=845.346561, last_y=905.048431,last_a=45.000000\n",
      "x=853.520715 y=913.222585 True_angle=60.880173 Des_Ro_dis=48.323500 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=-15.880173 Robot_a=45\n",
      "0.813684631723\n",
      "208498\n",
      "False_epoche=144\n",
      "**********************************\n",
      "last_x=862.530333, last_y=858.869790,last_a=90.000000\n",
      "x=862.530333 y=870.429790 True_angle=51.720300 Des_Ro_dis=47.732309 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=38.279700 Robot_a=90\n",
      "0.813367238983\n",
      "208893\n",
      "False_epoche=145\n",
      "**********************************\n",
      "last_x=899.012939, last_y=841.641426,last_a=135.000000\n",
      "x=890.838784 y=849.815580 True_angle=55.345485 Des_Ro_dis=51.013762 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=79.654515 Robot_a=135\n",
      "0.813017047214\n",
      "209329\n",
      "False_epoche=146\n",
      "**********************************\n",
      "last_x=847.739947, last_y=876.228182,last_a=45.000000\n",
      "x=855.914102 y=884.402336 True_angle=25.516136 Des_Ro_dis=46.763806 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=19.483864 Robot_a=45\n",
      "0.812037161741\n",
      "210550\n",
      "False_epoche=147\n",
      "**********************************\n",
      "last_x=848.316658, last_y=884.685489,last_a=315.000000\n",
      "x=856.490812 y=876.511335 True_angle=73.362627 Des_Ro_dis=49.444583 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=28.362627 Robot_a=315\n",
      "0.811575321359\n",
      "211126\n",
      "False_epoche=148\n",
      "**********************************\n",
      "last_x=887.645174, last_y=842.519391,last_a=90.000000\n",
      "x=887.645174 y=854.079391 True_angle=15.058697 Des_Ro_dis=47.553592 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=74.941303 Robot_a=90\n",
      "0.809799397524\n",
      "213344\n",
      "False_epoche=149\n",
      "**********************************\n",
      "last_x=844.394787, last_y=889.775804,last_a=45.000000\n",
      "x=852.568941 y=897.949959 True_angle=42.525131 Des_Ro_dis=47.475341 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=2.474869 Robot_a=45\n",
      "0.809314066602\n",
      "213951\n",
      "False_epoche=150\n",
      "**********************************\n",
      "last_x=853.726813, last_y=861.067907,last_a=45.000000\n",
      "x=861.900968 y=869.242062 True_angle=6.085525 Des_Ro_dis=48.965161 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=38.914475 Robot_a=45\n",
      "0.809206965693\n",
      "214085\n",
      "False_epoche=151\n",
      "**********************************\n",
      "last_x=851.127300, last_y=858.311831,last_a=45.000000\n",
      "x=859.301455 y=866.485985 True_angle=5.529582 Des_Ro_dis=52.721540 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=39.470418 Robot_a=45\n",
      "0.808953657237\n",
      "214402\n",
      "False_epoche=152\n",
      "**********************************\n",
      "last_x=855.000454, last_y=866.212547,last_a=45.000000\n",
      "x=863.174609 y=874.386702 True_angle=10.180088 Des_Ro_dis=44.857001 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=34.819912 Robot_a=45\n",
      "0.808472033405\n",
      "215005\n",
      "False_epoche=153\n",
      "**********************************\n",
      "last_x=864.782151, last_y=857.897568,last_a=45.000000\n",
      "x=872.956305 y=866.071723 True_angle=6.442203 Des_Ro_dis=43.387665 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=51.442203 Robot_a=45\n",
      "0.80809444544\n",
      "215478\n",
      "False_epoche=154\n",
      "**********************************\n",
      "last_x=852.920875, last_y=866.011608,last_a=0.000000\n",
      "x=864.480875 y=866.011608 True_angle=43.738404 Des_Ro_dis=49.161154 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=43.738404 Robot_a=0\n",
      "0.807927661131\n",
      "215687\n",
      "False_epoche=155\n",
      "**********************************\n",
      "last_x=876.754878, last_y=844.789993,last_a=90.000000\n",
      "x=876.754878 y=856.349993 True_angle=28.036828 Des_Ro_dis=49.453602 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=61.963172 Robot_a=90\n",
      "0.807378078325\n",
      "216376\n",
      "False_epoche=156\n",
      "**********************************\n",
      "last_x=864.952738, last_y=856.953431,last_a=45.000000\n",
      "x=873.126892 y=865.127585 True_angle=7.381715 Des_Ro_dis=44.025552 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=52.381715 Robot_a=45\n",
      "0.807246521796\n",
      "216541\n",
      "False_epoche=157\n",
      "**********************************\n",
      "last_x=873.147023, last_y=850.790125,last_a=0.000000\n",
      "x=884.707023 y=850.790125 True_angle=72.736297 Des_Ro_dis=51.531417 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=72.736297 Robot_a=0\n",
      "0.806486311292\n",
      "217495\n",
      "False_epoche=158\n",
      "**********************************\n",
      "last_x=865.478753, last_y=848.391127,last_a=45.000000\n",
      "x=873.652907 y=856.565281 True_angle=13.759393 Des_Ro_dis=50.801024 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=58.759393 Robot_a=45\n",
      "0.806252178736\n",
      "217789\n",
      "False_epoche=159\n",
      "**********************************\n",
      "last_x=878.195311, last_y=843.370096,last_a=45.000000\n",
      "x=886.369466 y=851.544251 True_angle=29.288782 Des_Ro_dis=50.336379 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=74.288782 Robot_a=45\n",
      "0.806167780478\n",
      "217895\n",
      "False_epoche=160\n",
      "**********************************\n",
      "last_x=868.789451, last_y=848.454831,last_a=45.000000\n",
      "x=876.963605 y=856.628986 True_angle=17.025117 Des_Ro_dis=49.109270 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=62.025117 Robot_a=45\n",
      "0.805791282167\n",
      "218368\n",
      "False_epoche=161\n",
      "**********************************\n",
      "last_x=869.792994, last_y=852.821255,last_a=90.000000\n",
      "x=869.792994 y=864.381255 True_angle=40.300106 Des_Ro_dis=46.702871 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=49.699894 Robot_a=90\n",
      "0.805503257858\n",
      "218730\n",
      "False_epoche=162\n",
      "**********************************\n",
      "last_x=842.292274, last_y=897.355893,last_a=0.000000\n",
      "x=853.852274 y=897.355893 True_angle=3.279267 Des_Ro_dis=46.223414 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=3.279267 Robot_a=0\n",
      "0.804307720228\n",
      "220234\n",
      "False_epoche=163\n",
      "**********************************\n",
      "last_x=900.489179, last_y=844.320382,last_a=45.000000\n",
      "x=908.663334 y=852.494537 True_angle=55.335171 Des_Ro_dis=48.288947 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=-79.664829 Robot_a=45\n",
      "0.803795556939\n",
      "220879\n",
      "False_epoche=164\n",
      "**********************************\n",
      "last_x=866.218241, last_y=854.782069,last_a=90.000000\n",
      "x=866.218241 y=866.342069 True_angle=45.105202 Des_Ro_dis=47.687143 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=44.894798 Robot_a=90\n",
      "0.803533647623\n",
      "221209\n",
      "False_epoche=165\n",
      "**********************************\n",
      "last_x=877.498565, last_y=846.434826,last_a=45.000000\n",
      "x=885.672720 y=854.608980 True_angle=27.482152 Des_Ro_dis=47.598484 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=72.482152 Robot_a=45\n",
      "0.803305142838\n",
      "221497\n",
      "False_epoche=166\n",
      "**********************************\n",
      "last_x=893.718564, last_y=837.636477,last_a=90.000000\n",
      "x=893.718564 y=849.196477 True_angle=7.048378 Des_Ro_dis=51.190374 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=82.951622 Robot_a=90\n",
      "0.803051325805\n",
      "221817\n",
      "False_epoche=167\n",
      "**********************************\n",
      "last_x=851.533924, last_y=865.202556,last_a=0.000000\n",
      "x=863.093924 y=865.202556 True_angle=43.315551 Des_Ro_dis=50.723964 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=43.315551 Robot_a=0\n",
      "0.802847537803\n",
      "222074\n",
      "False_epoche=168\n",
      "**********************************\n",
      "last_x=848.621778, last_y=874.275844,last_a=45.000000\n",
      "x=856.795933 y=882.449998 True_angle=22.892430 Des_Ro_dis=46.632542 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=22.107570 Robot_a=45\n",
      "0.802811067655\n",
      "222120\n",
      "False_epoche=169\n",
      "**********************************\n",
      "last_x=847.896651, last_y=862.436882,last_a=45.000000\n",
      "x=856.070806 y=870.611037 True_angle=11.217178 Des_Ro_dis=52.853432 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=33.782822 Robot_a=45\n",
      "0.801967965289\n",
      "223184\n",
      "False_epoche=170\n",
      "**********************************\n",
      "last_x=841.899543, last_y=893.876869,last_a=45.000000\n",
      "x=850.073698 y=902.051023 True_angle=47.352446 Des_Ro_dis=49.968414 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=-2.352446 Robot_a=45\n",
      "0.801812754779\n",
      "223380\n",
      "False_epoche=171\n",
      "**********************************\n",
      "last_x=878.596830, last_y=848.180729,last_a=0.000000\n",
      "x=890.156830 y=848.180729 True_angle=79.244691 Des_Ro_dis=52.745852 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=79.244691 Robot_a=0\n",
      "0.801667866302\n",
      "223563\n",
      "False_epoche=172\n",
      "**********************************\n",
      "last_x=852.049230, last_y=867.555199,last_a=0.000000\n",
      "x=863.609230 y=867.555199 True_angle=41.719135 Des_Ro_dis=48.754007 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=41.719135 Robot_a=0\n",
      "0.801545958838\n",
      "223717\n",
      "False_epoche=173\n",
      "**********************************\n",
      "last_x=895.582905, last_y=844.583068,last_a=135.000000\n",
      "x=887.408750 y=852.757222 True_angle=59.923690 Des_Ro_dis=48.891918 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=75.076310 Robot_a=135\n",
      "0.80119142567\n",
      "224165\n",
      "False_epoche=174\n",
      "**********************************\n",
      "last_x=887.225508, last_y=838.185286,last_a=45.000000\n",
      "x=895.399663 y=846.359440 True_angle=40.098177 Des_Ro_dis=53.837466 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=85.098177 Robot_a=45\n",
      "0.800465445217\n",
      "225083\n",
      "False_epoche=175\n",
      "**********************************\n",
      "last_x=896.413446, last_y=838.488779,last_a=90.000000\n",
      "x=896.413446 y=850.048779 True_angle=4.106854 Des_Ro_dis=50.079815 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=85.893146 Robot_a=90\n",
      "0.799860970393\n",
      "225848\n",
      "False_epoche=176\n",
      "**********************************\n",
      "last_x=856.376874, last_y=859.763805,last_a=45.000000\n",
      "x=864.551028 y=867.937960 True_angle=2.872040 Des_Ro_dis=47.797531 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=42.127960 Robot_a=45\n",
      "0.799268007489\n",
      "226599\n",
      "False_epoche=177\n",
      "**********************************\n",
      "last_x=877.043562, last_y=849.686986,last_a=45.000000\n",
      "x=885.217717 y=857.861140 True_angle=25.669164 Des_Ro_dis=44.656460 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=70.669164 Robot_a=45\n",
      "0.798935006658\n",
      "227021\n",
      "False_epoche=178\n",
      "**********************************\n",
      "last_x=849.550606, last_y=886.670801,last_a=45.000000\n",
      "x=857.724760 y=894.844955 True_angle=38.047673 Des_Ro_dis=42.588383 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=6.952327 Robot_a=45\n",
      "0.79861634129\n",
      "227425\n",
      "False_epoche=179\n",
      "**********************************\n",
      "last_x=868.660078, last_y=847.070475,last_a=45.000000\n",
      "x=876.834232 y=855.244630 True_angle=17.633524 Des_Ro_dis=50.395397 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=62.633524 Robot_a=45\n",
      "0.798502788713\n",
      "227569\n",
      "False_epoche=180\n",
      "**********************************\n",
      "last_x=843.400192, last_y=873.872593,last_a=45.000000\n",
      "x=851.574346 y=882.046748 True_angle=24.658319 Des_Ro_dis=51.646522 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=20.341681 Robot_a=45\n",
      "0.798373484859\n",
      "227733\n",
      "False_epoche=181\n",
      "**********************************\n",
      "last_x=849.046747, last_y=886.024146,last_a=0.000000\n",
      "x=860.606747 y=886.024146 True_angle=19.533558 Des_Ro_dis=41.798958 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=19.533558 Robot_a=0\n",
      "0.797914784985\n",
      "228315\n",
      "False_epoche=182\n",
      "**********************************\n",
      "last_x=859.270230, last_y=850.878553,last_a=45.000000\n",
      "x=867.444384 y=859.052707 True_angle=6.513142 Des_Ro_dis=52.312034 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=51.513142 Robot_a=45\n",
      "0.797596531704\n",
      "228719\n",
      "False_epoche=183\n",
      "**********************************\n",
      "last_x=842.770657, last_y=897.475788,last_a=0.000000\n",
      "x=854.330657 y=897.475788 True_angle=3.163603 Des_Ro_dis=45.739049 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=3.163603 Robot_a=0\n",
      "0.797189449537\n",
      "229236\n",
      "False_epoche=184\n",
      "**********************************\n",
      "last_x=835.207466, last_y=902.020695,last_a=0.000000\n",
      "x=846.767466 y=902.020695 True_angle=2.173892 Des_Ro_dis=53.270873 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-2.173892 Robot_a=0\n",
      "0.796814836524\n",
      "229712\n",
      "False_epoche=185\n",
      "**********************************\n",
      "last_x=847.114912, last_y=888.730907,last_a=0.000000\n",
      "x=858.674912 y=888.730907 True_angle=15.253331 Des_Ro_dis=42.834044 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=15.253331 Robot_a=0\n",
      "0.7964608495\n",
      "230162\n",
      "False_epoche=186\n",
      "**********************************\n",
      "last_x=873.925262, last_y=846.659446,last_a=45.000000\n",
      "x=882.099416 y=854.833601 True_angle=23.380328 Des_Ro_dis=48.584303 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=68.380328 Robot_a=45\n",
      "0.796302786755\n",
      "230363\n",
      "False_epoche=187\n",
      "**********************************\n",
      "last_x=842.096465, last_y=887.229718,last_a=45.000000\n",
      "x=850.270619 y=895.403872 True_angle=39.719565 Des_Ro_dis=49.941322 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=5.280435 Robot_a=45\n",
      "0.79627369409\n",
      "230400\n",
      "False_epoche=188\n",
      "**********************************\n",
      "last_x=852.538411, last_y=867.029571,last_a=90.000000\n",
      "x=852.538411 y=878.589571 True_angle=65.719356 Des_Ro_dis=52.067350 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=24.280644 Robot_a=90\n",
      "0.795956889632\n",
      "230803\n",
      "False_epoche=189\n",
      "**********************************\n",
      "last_x=840.703762, last_y=913.066300,last_a=315.000000\n",
      "x=848.877916 y=904.892146 True_angle=39.533706 Des_Ro_dis=51.355628 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=-5.466294 Robot_a=315\n",
      "0.795591504637\n",
      "231268\n",
      "False_epoche=190\n",
      "**********************************\n",
      "last_x=848.908761, last_y=869.282897,last_a=45.000000\n",
      "x=857.082916 y=877.457051 True_angle=17.288515 Des_Ro_dis=48.477424 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=27.711485 Robot_a=45\n",
      "0.794869092813\n",
      "232188\n",
      "False_epoche=191\n",
      "**********************************\n",
      "last_x=863.623389, last_y=853.038329,last_a=45.000000\n",
      "x=871.797544 y=861.212483 True_angle=8.979049 Des_Ro_dis=47.956751 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=53.979049 Robot_a=45\n",
      "0.794506567042\n",
      "232650\n",
      "False_epoche=192\n",
      "**********************************\n",
      "last_x=839.094273, last_y=881.085650,last_a=0.000000\n",
      "x=850.654273 y=881.085650 True_angle=20.971963 Des_Ro_dis=52.846508 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=20.971963 Robot_a=0\n",
      "0.794206158559\n",
      "233033\n",
      "False_epoche=193\n",
      "**********************************\n",
      "last_x=884.305874, last_y=846.482062,last_a=90.000000\n",
      "x=884.305874 y=858.042062 True_angle=20.508021 Des_Ro_dis=44.797033 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=69.491979 Robot_a=90\n",
      "0.793866670797\n",
      "233466\n",
      "False_epoche=194\n",
      "**********************************\n",
      "last_x=882.804020, last_y=843.652076,last_a=90.000000\n",
      "x=882.804020 y=855.212076 True_angle=21.003910 Des_Ro_dis=47.975617 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=68.996090 Robot_a=90\n",
      "0.793545351337\n",
      "233876\n",
      "False_epoche=195\n",
      "**********************************\n",
      "last_x=886.367670, last_y=848.454557,last_a=45.000000\n",
      "x=894.541825 y=856.628711 True_angle=37.827168 Des_Ro_dis=43.713389 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=82.827168 Robot_a=45\n",
      "0.793313456235\n",
      "234172\n",
      "False_epoche=196\n",
      "**********************************\n",
      "last_x=872.200882, last_y=845.931891,last_a=90.000000\n",
      "x=872.200882 y=857.491891 True_angle=33.183567 Des_Ro_dis=50.791046 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=56.816433 Robot_a=90\n",
      "0.793206932848\n",
      "234308\n",
      "False_epoche=197\n",
      "**********************************\n",
      "last_x=887.391978, last_y=843.391121,last_a=135.000000\n",
      "x=879.217824 y=851.565275 True_angle=68.223008 Des_Ro_dis=52.705042 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=66.776992 Robot_a=135\n",
      "0.792671402479\n",
      "234992\n",
      "False_epoche=198\n",
      "**********************************\n",
      "last_x=877.050177, last_y=845.309475,last_a=45.000000\n",
      "x=885.224331 y=853.483630 True_angle=27.377836 Des_Ro_dis=48.806691 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=72.377836 Robot_a=45\n",
      "0.792190989739\n",
      "235606\n",
      "False_epoche=199\n",
      "**********************************\n",
      "last_x=860.996312, last_y=857.623080,last_a=0.000000\n",
      "x=872.556312 y=857.623080 True_angle=57.072530 Des_Ro_dis=50.487220 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=57.072530 Robot_a=0\n",
      "0.791780447247\n",
      "236131\n",
      "False_epoche=200\n",
      "**********************************\n",
      "last_x=844.691494, last_y=870.571317,last_a=45.000000\n",
      "x=852.865648 y=878.745472 True_angle=20.727721 Des_Ro_dis=51.704952 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=24.272279 Robot_a=45\n",
      "0.791683512481\n",
      "236255\n",
      "False_epoche=201\n",
      "**********************************\n",
      "last_x=844.270592, last_y=875.025782,last_a=0.000000\n",
      "x=855.830592 y=875.025782 True_angle=29.484638 Des_Ro_dis=50.740992 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=29.484638 Robot_a=0\n",
      "0.791193550538\n",
      "236882\n",
      "False_epoche=202\n",
      "**********************************\n",
      "last_x=890.939658, last_y=845.555095,last_a=45.000000\n",
      "x=899.113812 y=853.729249 True_angle=43.902793 Des_Ro_dis=46.279236 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=88.902793 Robot_a=45\n",
      "0.790946732375\n",
      "237198\n",
      "False_epoche=203\n",
      "**********************************\n",
      "last_x=876.615309, last_y=844.318413,last_a=90.000000\n",
      "x=876.615309 y=855.878413 True_angle=27.923843 Des_Ro_dis=49.935540 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=62.076157 Robot_a=90\n",
      "0.790667203484\n",
      "237556\n",
      "False_epoche=204\n",
      "**********************************\n",
      "last_x=847.868869, last_y=869.965796,last_a=0.000000\n",
      "x=859.428869 y=869.965796 True_angle=36.511995 Des_Ro_dis=50.478412 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=36.511995 Robot_a=0\n",
      "0.790582895978\n",
      "237664\n",
      "False_epoche=205\n",
      "**********************************\n",
      "last_x=885.572491, last_y=849.644880,last_a=90.000000\n",
      "x=885.572491 y=861.204880 True_angle=20.399654 Des_Ro_dis=41.390994 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=69.600346 Robot_a=90\n",
      "0.790085823008\n",
      "238301\n",
      "False_epoche=206\n",
      "**********************************\n",
      "last_x=876.211729, last_y=844.726005,last_a=45.000000\n",
      "x=884.385883 y=852.900160 True_angle=26.659072 Des_Ro_dis=49.620516 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=71.659072 Robot_a=45\n",
      "0.789995338301\n",
      "238417\n",
      "False_epoche=207\n",
      "**********************************\n",
      "last_x=885.900344, last_y=847.087889,last_a=135.000000\n",
      "x=877.726190 y=855.262044 True_angle=71.467463 Des_Ro_dis=49.976068 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=63.532537 Robot_a=135\n",
      "0.789719268818\n",
      "238771\n",
      "False_epoche=208\n",
      "**********************************\n",
      "last_x=858.968565, last_y=855.718732,last_a=90.000000\n",
      "x=858.968565 y=867.278732 True_angle=51.428750 Des_Ro_dis=52.481045 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=38.571250 Robot_a=90\n",
      "0.789351328164\n",
      "239243\n",
      "False_epoche=209\n",
      "**********************************\n",
      "last_x=861.132289, last_y=851.384501,last_a=45.000000\n",
      "x=869.306444 y=859.558656 True_angle=7.802782 Des_Ro_dis=50.770038 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=52.802782 Robot_a=45\n",
      "0.788457924656\n",
      "240390\n",
      "False_epoche=210\n",
      "**********************************\n",
      "last_x=852.525002, last_y=869.812419,last_a=45.000000\n",
      "x=860.699156 y=877.986574 True_angle=15.745676 Des_Ro_dis=45.046057 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=29.254324 Robot_a=45\n",
      "0.788417445896\n",
      "240442\n",
      "False_epoche=211\n",
      "**********************************\n",
      "last_x=907.839168, last_y=843.708069,last_a=135.000000\n",
      "x=899.665013 y=851.882224 True_angle=45.398876 Des_Ro_dis=48.118942 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=89.601124 Robot_a=135\n",
      "0.787919418117\n",
      "241082\n",
      "False_epoche=212\n",
      "**********************************\n",
      "last_x=876.951957, last_y=848.895714,last_a=45.000000\n",
      "x=885.126111 y=857.069868 True_angle=25.890469 Des_Ro_dis=45.433785 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=70.890469 Robot_a=45\n",
      "0.787616088697\n",
      "241472\n",
      "False_epoche=213\n",
      "**********************************\n",
      "last_x=859.653024, last_y=859.797070,last_a=45.000000\n",
      "x=867.827179 y=867.971225 True_angle=0.128552 Des_Ro_dis=45.397499 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=44.871448 Robot_a=45\n",
      "0.787413934795\n",
      "241732\n",
      "False_epoche=214\n",
      "**********************************\n",
      "last_x=864.208797, last_y=844.874135,last_a=45.000000\n",
      "x=872.382952 y=853.048289 True_angle=14.535910 Des_Ro_dis=54.471685 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=59.535910 Robot_a=45\n",
      "0.786938303025\n",
      "242344\n",
      "False_epoche=215\n",
      "**********************************\n",
      "last_x=841.655173, last_y=906.566533,last_a=45.000000\n",
      "x=849.829328 y=914.740688 True_angle=61.373360 Des_Ro_dis=52.291340 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=-16.373360 Robot_a=45\n",
      "0.786845852868\n",
      "242463\n",
      "False_epoche=216\n",
      "**********************************\n",
      "last_x=886.962883, last_y=844.233693,last_a=135.000000\n",
      "x=878.788729 y=852.407848 True_angle=69.021979 Des_Ro_dis=52.104999 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=65.978021 Robot_a=135\n",
      "0.78624713555\n",
      "243234\n",
      "False_epoche=217\n",
      "**********************************\n",
      "last_x=852.502289, last_y=870.691982,last_a=90.000000\n",
      "x=852.502289 y=882.251982 True_angle=69.511270 Des_Ro_dis=50.705273 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=20.488730 Robot_a=90\n",
      "0.785232468034\n",
      "244542\n",
      "False_epoche=218\n",
      "**********************************\n",
      "last_x=841.749581, last_y=899.915047,last_a=0.000000\n",
      "x=853.309581 y=899.915047 True_angle=0.104249 Des_Ro_dis=46.690496 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=0.104249 Robot_a=0\n",
      "0.785003033182\n",
      "244838\n",
      "False_epoche=219\n",
      "**********************************\n",
      "last_x=878.566328, last_y=849.513908,last_a=90.000000\n",
      "x=878.566328 y=861.073908 True_angle=28.838271 Des_Ro_dis=44.436955 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=61.161729 Robot_a=90\n",
      "0.784747324375\n",
      "245168\n",
      "False_epoche=220\n",
      "**********************************\n",
      "last_x=895.193086, last_y=841.130674,last_a=45.000000\n",
      "x=903.367241 y=849.304828 True_angle=48.800080 Des_Ro_dis=50.806877 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=-86.199920 Robot_a=45\n",
      "0.784470014474\n",
      "245526\n",
      "False_epoche=221\n",
      "**********************************\n",
      "last_x=867.314802, last_y=844.023738,last_a=45.000000\n",
      "x=875.488957 y=852.197893 True_angle=17.853031 Des_Ro_dis=53.719947 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=62.853031 Robot_a=45\n",
      "0.784312813018\n",
      "245729\n",
      "False_epoche=222\n",
      "**********************************\n",
      "last_x=882.870127, last_y=848.631675,last_a=0.000000\n",
      "x=894.430127 y=848.631675 True_angle=83.811589 Des_Ro_dis=51.669414 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=83.811589 Robot_a=0\n",
      "0.783523416609\n",
      "246749\n",
      "False_epoche=223\n",
      "**********************************\n",
      "last_x=905.309793, last_y=843.983132,last_a=90.000000\n",
      "x=905.309793 y=855.543132 True_angle=6.810970 Des_Ro_dis=44.772838 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-83.189030 Robot_a=90\n",
      "0.783156080236\n",
      "247224\n",
      "False_epoche=224\n",
      "**********************************\n",
      "last_x=862.782450, last_y=851.761009,last_a=0.000000\n",
      "x=874.342450 y=851.761009 True_angle=61.992200 Des_Ro_dis=54.637992 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=61.992200 Robot_a=0\n",
      "0.782990642537\n",
      "247438\n",
      "False_epoche=225\n",
      "**********************************\n",
      "last_x=901.527805, last_y=840.701460,last_a=45.000000\n",
      "x=909.701959 y=848.875614 True_angle=55.745339 Des_Ro_dis=52.036822 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=-79.254661 Robot_a=45\n",
      "0.782333879655\n",
      "248288\n",
      "False_epoche=226\n",
      "**********************************\n",
      "last_x=834.896652, last_y=897.144933,last_a=0.000000\n",
      "x=846.456652 y=897.144933 True_angle=3.052266 Des_Ro_dis=53.619414 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=3.052266 Robot_a=0\n",
      "0.781032821833\n",
      "249974\n",
      "False_epoche=227\n",
      "**********************************\n",
      "last_x=861.823928, last_y=855.150037,last_a=45.000000\n",
      "x=869.998082 y=863.324192 True_angle=5.715798 Des_Ro_dis=47.383858 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=50.715798 Robot_a=45\n",
      "0.780547994683\n",
      "250603\n",
      "False_epoche=228\n",
      "**********************************\n",
      "last_x=864.041944, last_y=855.865419,last_a=45.000000\n",
      "x=872.216098 y=864.039574 True_angle=7.309451 Des_Ro_dis=45.443344 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=52.309451 Robot_a=45\n",
      "0.780130470829\n",
      "251145\n",
      "False_epoche=229\n",
      "**********************************\n",
      "last_x=875.533191, last_y=846.239616,last_a=45.000000\n",
      "x=883.707345 y=854.413770 True_angle=25.332958 Des_Ro_dis=48.410277 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=70.332958 Robot_a=45\n",
      "0.779817861322\n",
      "251551\n",
      "False_epoche=230\n",
      "**********************************\n",
      "last_x=871.379988, last_y=842.036324,last_a=45.000000\n",
      "x=879.554142 y=850.210478 True_angle=22.674702 Des_Ro_dis=53.824062 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=67.674702 Robot_a=45\n",
      "0.779028433242\n",
      "252577\n",
      "False_epoche=231\n",
      "**********************************\n",
      "last_x=874.457671, last_y=850.348576,last_a=90.000000\n",
      "x=874.457671 y=861.908576 True_angle=33.843959 Des_Ro_dis=45.862481 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=56.156041 Robot_a=90\n",
      "0.777853487242\n",
      "254106\n",
      "False_epoche=232\n",
      "**********************************\n",
      "last_x=850.106653, last_y=870.092285,last_a=90.000000\n",
      "x=850.106653 y=881.652285 True_angle=69.809571 Des_Ro_dis=53.159991 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=20.190429 Robot_a=90\n",
      "0.777464284196\n",
      "254613\n",
      "False_epoche=233\n",
      "**********************************\n",
      "last_x=865.838547, last_y=854.448777,last_a=45.000000\n",
      "x=874.012701 y=862.622931 True_angle=10.190121 Des_Ro_dis=45.523455 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=55.190121 Robot_a=45\n",
      "0.777218734915\n",
      "254933\n",
      "False_epoche=234\n",
      "**********************************\n",
      "last_x=876.717366, last_y=843.069693,last_a=45.000000\n",
      "x=884.891520 y=851.243848 True_angle=27.782919 Des_Ro_dis=51.043399 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=72.782919 Robot_a=45\n",
      "0.776471061198\n",
      "255908\n",
      "False_epoche=235\n",
      "**********************************\n",
      "last_x=860.981291, last_y=858.853694,last_a=0.000000\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "x=872.541291 y=858.853694 True_angle=56.283104 Des_Ro_dis=49.467153 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=56.283104 Robot_a=0\n",
      "0.776343071219\n",
      "256075\n",
      "False_epoche=236\n",
      "**********************************\n",
      "last_x=860.645072, last_y=860.627580,last_a=45.000000\n",
      "x=868.819227 y=868.801734 True_angle=0.016067 Des_Ro_dis=44.108643 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=45.016067 Robot_a=45\n",
      "0.775384210066\n",
      "257327\n",
      "False_epoche=237\n",
      "**********************************\n",
      "last_x=878.846482, last_y=846.822511,last_a=90.000000\n",
      "x=878.846482 y=858.382511 True_angle=26.943518 Des_Ro_dis=46.684973 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=63.056482 Robot_a=90\n",
      "0.775378852395\n",
      "257334\n",
      "False_epoche=238\n",
      "**********************************\n",
      "last_x=864.385161, last_y=851.174633,last_a=0.000000\n",
      "x=875.945161 y=851.174633 True_angle=63.771872 Des_Ro_dis=54.429328 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=63.771872 Robot_a=0\n",
      "0.775131674947\n",
      "257657\n",
      "False_epoche=239\n",
      "**********************************\n",
      "last_x=853.874489, last_y=865.155337,last_a=45.000000\n",
      "x=862.048644 y=873.329491 True_angle=9.902174 Des_Ro_dis=46.385574 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=35.097826 Robot_a=45\n",
      "0.774437250702\n",
      "258565\n",
      "False_epoche=240\n",
      "**********************************\n",
      "last_x=868.165487, last_y=851.095704,last_a=0.000000\n",
      "x=879.725487 y=851.095704 True_angle=67.482312 Des_Ro_dis=52.940401 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=67.482312 Robot_a=0\n",
      "0.77429049284\n",
      "258757\n",
      "False_epoche=241\n",
      "**********************************\n",
      "last_x=844.260884, last_y=885.625311,last_a=0.000000\n",
      "x=855.820884 y=885.625311 True_angle=18.023487 Des_Ro_dis=46.458863 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=18.023487 Robot_a=0\n",
      "0.774092567234\n",
      "259016\n",
      "False_epoche=242\n",
      "**********************************\n",
      "last_x=864.927648, last_y=857.584174,last_a=45.000000\n",
      "x=873.101803 y=865.758329 True_angle=6.848954 Des_Ro_dis=43.543140 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=51.848954 Robot_a=45\n",
      "0.773172390209\n",
      "260221\n",
      "False_epoche=243\n",
      "**********************************\n",
      "last_x=865.211865, last_y=859.615743,last_a=45.000000\n",
      "x=873.386020 y=867.789897 True_angle=5.434375 Des_Ro_dis=41.782708 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=50.434375 Robot_a=45\n",
      "0.773020534013\n",
      "260420\n",
      "False_epoche=244\n",
      "**********************************\n",
      "last_x=843.187582, last_y=885.116530,last_a=0.000000\n",
      "x=854.747582 y=885.116530 True_angle=18.205978 Des_Ro_dis=47.637160 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=18.205978 Robot_a=0\n",
      "0.772898460494\n",
      "260580\n",
      "False_epoche=245\n",
      "**********************************\n",
      "last_x=853.596807, last_y=867.057688,last_a=90.000000\n",
      "x=853.596807 y=878.617688 True_angle=65.260035 Des_Ro_dis=51.092657 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=24.739965 Robot_a=90\n",
      "0.772270091086\n",
      "261404\n",
      "False_epoche=246\n",
      "**********************************\n",
      "last_x=848.166115, last_y=867.329466,last_a=45.000000\n",
      "x=856.340270 y=875.503620 True_angle=15.704310 Des_Ro_dis=50.062408 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=29.295690 Robot_a=45\n",
      "0.772153472684\n",
      "261557\n",
      "False_epoche=247\n",
      "**********************************\n",
      "last_x=844.982648, last_y=878.041368,last_a=45.000000\n",
      "x=853.156802 y=886.215522 True_angle=28.602504 Des_Ro_dis=48.829264 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=16.397496 Robot_a=45\n",
      "0.771970577799\n",
      "261797\n",
      "False_epoche=248\n",
      "**********************************\n",
      "last_x=851.466937, last_y=873.934439,last_a=45.000000\n",
      "x=859.641091 y=882.108594 True_angle=21.091932 Des_Ro_dis=44.146845 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=23.908068 Robot_a=45\n",
      "0.771602634875\n",
      "262280\n",
      "False_epoche=249\n",
      "**********************************\n",
      "last_x=839.979158, last_y=915.280815,last_a=0.000000\n",
      "x=851.539158 y=915.280815 True_angle=17.501259 Des_Ro_dis=50.812956 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-17.501259 Robot_a=0\n",
      "0.771486118586\n",
      "262433\n",
      "False_epoche=250\n",
      "**********************************\n",
      "last_x=892.451561, last_y=845.548867,last_a=90.000000\n",
      "x=892.451561 y=857.108867 True_angle=9.981311 Des_Ro_dis=43.550295 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=80.018689 Robot_a=90\n",
      "0.771385609052\n",
      "262565\n",
      "False_epoche=251\n",
      "**********************************\n",
      "last_x=856.599261, last_y=865.848888,last_a=90.000000\n",
      "x=856.599261 y=877.408888 True_angle=62.501956 Des_Ro_dis=48.928340 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=27.498044 Robot_a=90\n",
      "0.771276738694\n",
      "262708\n",
      "False_epoche=252\n",
      "**********************************\n",
      "last_x=878.793322, last_y=841.935709,last_a=90.000000\n",
      "x=878.793322 y=853.495709 True_angle=24.513722 Des_Ro_dis=51.111371 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=65.486278 Robot_a=90\n",
      "0.771106992909\n",
      "262931\n",
      "False_epoche=253\n",
      "**********************************\n",
      "last_x=835.723338, last_y=904.860574,last_a=0.000000\n",
      "x=847.283338 y=904.860574 True_angle=5.267883 Des_Ro_dis=52.940265 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-5.267883 Robot_a=0\n",
      "0.770774461816\n",
      "263368\n",
      "False_epoche=254\n",
      "**********************************\n",
      "last_x=878.462612, last_y=847.080277,last_a=45.000000\n",
      "x=886.636767 y=855.254432 True_angle=28.371781 Des_Ro_dis=46.698414 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=73.371781 Robot_a=45\n",
      "0.770399492444\n",
      "263861\n",
      "False_epoche=255\n",
      "**********************************\n",
      "last_x=861.060702, last_y=860.674599,last_a=45.000000\n",
      "x=869.234857 y=868.848753 True_angle=0.357285 Des_Ro_dis=43.782350 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=45.357285 Robot_a=45\n",
      "0.770323456297\n",
      "263961\n",
      "False_epoche=256\n",
      "**********************************\n",
      "last_x=837.885555, last_y=891.762427,last_a=0.000000\n",
      "x=849.445555 y=891.762427 True_angle=9.254702 Des_Ro_dis=51.221183 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=9.254702 Robot_a=0\n",
      "0.770216258247\n",
      "264102\n",
      "False_epoche=257\n",
      "**********************************\n",
      "last_x=885.522266, last_y=849.027750,last_a=90.000000\n",
      "x=885.522266 y=860.587750 True_angle=20.170352 Des_Ro_dis=41.987263 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=69.829648 Robot_a=90\n",
      "0.770036868379\n",
      "264338\n",
      "False_epoche=258\n",
      "**********************************\n",
      "last_x=855.482645, last_y=867.101003,last_a=45.000000\n",
      "x=863.656799 y=875.275158 True_angle=10.771939 Des_Ro_dis=43.956184 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=34.228061 Robot_a=45\n",
      "0.769763304351\n",
      "264698\n",
      "False_epoche=259\n",
      "**********************************\n",
      "last_x=875.538695, last_y=850.626885,last_a=90.000000\n",
      "x=875.538695 y=862.186885 True_angle=32.898830 Des_Ro_dis=45.035398 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=57.101170 Robot_a=90\n",
      "0.769520218989\n",
      "265018\n",
      "False_epoche=260\n",
      "**********************************\n",
      "last_x=857.454141, last_y=865.402656,last_a=0.000000\n",
      "x=869.014141 y=865.402656 True_angle=48.151936 Des_Ro_dis=46.444587 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=48.151936 Robot_a=0\n",
      "0.769289359935\n",
      "265322\n",
      "False_epoche=261\n",
      "**********************************\n",
      "last_x=864.399742, last_y=859.750354,last_a=0.000000\n",
      "x=875.959742 y=859.750354 True_angle=59.151026 Des_Ro_dis=46.882492 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=59.151026 Robot_a=0\n",
      "0.768734522231\n",
      "266053\n",
      "False_epoche=262\n",
      "**********************************\n",
      "last_x=850.353388, last_y=868.225173,last_a=45.000000\n",
      "x=858.527542 y=876.399328 True_angle=15.357156 Des_Ro_dis=47.717465 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=29.642844 Robot_a=45\n",
      "0.767623032342\n",
      "267519\n",
      "False_epoche=263\n",
      "**********************************\n",
      "last_x=846.465790, last_y=888.903288,last_a=0.000000\n",
      "x=858.025790 y=888.903288 True_angle=14.808481 Des_Ro_dis=43.416257 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=14.808481 Robot_a=0\n",
      "0.767356395966\n",
      "267871\n",
      "False_epoche=264\n",
      "**********************************\n",
      "last_x=853.074618, last_y=871.011792,last_a=90.000000\n",
      "x=853.074618 y=882.571792 True_angle=69.624873 Des_Ro_dis=50.057306 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=20.375127 Robot_a=90\n",
      "0.767099695661\n",
      "268210\n",
      "False_epoche=265\n",
      "**********************************\n",
      "last_x=844.595448, last_y=867.905149,last_a=45.000000\n",
      "x=852.769603 y=876.079304 True_angle=18.139234 Des_Ro_dis=52.942518 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=26.860766 Robot_a=45\n",
      "0.766622873067\n",
      "268840\n",
      "False_epoche=266\n",
      "**********************************\n",
      "last_x=889.510554, last_y=846.110318,last_a=90.000000\n",
      "x=889.510554 y=857.670318 True_angle=13.917738 Des_Ro_dis=43.609981 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=76.082262 Robot_a=90\n",
      "0.766585799455\n",
      "268889\n",
      "False_epoche=267\n",
      "**********************************\n",
      "last_x=845.428232, last_y=875.352251,last_a=0.000000\n",
      "x=856.988232 y=875.352251 True_angle=29.814720 Des_Ro_dis=49.573418 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=29.814720 Robot_a=0\n",
      "0.766365665018\n",
      "269180\n",
      "False_epoche=268\n",
      "**********************************\n",
      "last_x=840.493663, last_y=890.004426,last_a=45.000000\n",
      "x=848.667817 y=898.178581 True_angle=42.967827 Des_Ro_dis=51.364488 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=2.032173 Robot_a=45\n",
      "0.765638391123\n",
      "270142\n",
      "False_epoche=269\n",
      "**********************************\n",
      "last_x=871.836489, last_y=841.216729,last_a=90.000000\n",
      "x=871.836489 y=852.776729 True_angle=30.811450 Des_Ro_dis=54.983823 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=59.188550 Robot_a=90\n",
      "0.765219883392\n",
      "270696\n",
      "False_epoche=270\n",
      "**********************************\n",
      "last_x=849.122635, last_y=860.571473,last_a=45.000000\n",
      "x=857.296789 y=868.745627 True_angle=8.799697 Des_Ro_dis=52.918806 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=36.200303 Robot_a=45\n",
      "0.764948053166\n",
      "271056\n",
      "False_epoche=271\n",
      "**********************************\n",
      "last_x=851.570362, last_y=860.459990,last_a=45.000000\n",
      "x=859.744516 y=868.634144 True_angle=7.075356 Des_Ro_dis=51.032548 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=37.924644 Robot_a=45\n",
      "0.764328495178\n",
      "271877\n",
      "False_epoche=272\n",
      "**********************************\n",
      "last_x=842.689510, last_y=909.823171,last_a=0.000000\n",
      "x=854.249510 y=909.823171 True_angle=12.118097 Des_Ro_dis=46.793184 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-12.118097 Robot_a=0\n",
      "0.763916744238\n",
      "272423\n",
      "False_epoche=273\n",
      "**********************************\n",
      "last_x=838.349483, last_y=892.548746,last_a=45.000000\n",
      "x=846.523637 y=900.722901 True_angle=45.774485 Des_Ro_dis=53.481248 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=-0.774485 Robot_a=45\n",
      "0.762895125946\n",
      "273779\n",
      "False_epoche=274\n",
      "**********************************\n",
      "last_x=843.359515, last_y=897.692068,last_a=0.000000\n",
      "x=854.919515 y=897.692068 True_angle=2.930746 Des_Ro_dis=45.139525 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=2.930746 Robot_a=0\n",
      "0.762360007769\n",
      "274490\n",
      "False_epoche=275\n",
      "**********************************\n",
      "last_x=862.586956, last_y=853.240189,last_a=45.000000\n",
      "x=870.761110 y=861.414343 True_angle=7.846397 Des_Ro_dis=48.412453 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=52.846397 Robot_a=45\n",
      "0.762134333619\n",
      "274790\n",
      "False_epoche=276\n",
      "**********************************\n",
      "last_x=847.218505, last_y=880.512050,last_a=45.000000\n",
      "x=855.392660 y=888.686204 True_angle=30.768131 Des_Ro_dis=46.019743 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=14.231869 Robot_a=45\n",
      "0.761643350223\n",
      "275443\n",
      "False_epoche=277\n",
      "**********************************\n",
      "last_x=849.479204, last_y=874.377877,last_a=0.000000\n",
      "x=861.039204 y=874.377877 True_angle=33.330464 Des_Ro_dis=46.630857 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=33.330464 Robot_a=0\n",
      "0.761348012424\n",
      "275836\n",
      "False_epoche=278\n",
      "**********************************\n",
      "last_x=846.536713, last_y=869.113866,last_a=45.000000\n",
      "x=854.710868 y=877.288020 True_angle=18.366761 Des_Ro_dis=50.664973 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=26.633239 Robot_a=45\n",
      "0.761053541723\n",
      "276228\n",
      "False_epoche=279\n",
      "**********************************\n",
      "last_x=887.998114, last_y=848.887712,last_a=90.000000\n",
      "x=887.998114 y=860.447712 True_angle=16.880082 Des_Ro_dis=41.333143 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=73.119918 Robot_a=90\n",
      "0.760864300076\n",
      "276480\n",
      "False_epoche=280\n",
      "**********************************\n",
      "last_x=838.809697, last_y=890.373916,last_a=45.000000\n",
      "x=846.983851 y=898.548070 True_angle=43.431258 Des_Ro_dis=53.036026 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=1.568742 Robot_a=45\n",
      "0.760506223213\n",
      "276957\n",
      "False_epoche=281\n",
      "**********************************\n",
      "last_x=891.118112, last_y=844.594934,last_a=45.000000\n",
      "x=899.292266 y=852.769089 True_angle=44.141513 Des_Ro_dis=47.236213 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=89.141513 Robot_a=45\n",
      "0.760326873659\n",
      "277196\n",
      "False_epoche=282\n",
      "**********************************\n",
      "last_x=857.185215, last_y=863.362728,last_a=0.000000\n",
      "x=868.745215 y=863.362728 True_angle=49.532933 Des_Ro_dis=48.157567 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=49.532933 Robot_a=0\n",
      "0.759942054687\n",
      "277709\n",
      "False_epoche=283\n",
      "**********************************\n",
      "last_x=915.877738, last_y=843.992906,last_a=90.000000\n",
      "x=915.877738 y=855.552906 True_angle=19.658141 Des_Ro_dis=47.197953 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=-70.341859 Robot_a=90\n",
      "0.759816075005\n",
      "277877\n",
      "False_epoche=284\n",
      "**********************************\n",
      "last_x=906.263641, last_y=842.366571,last_a=45.000000\n",
      "x=914.437796 y=850.540725 True_angle=61.273201 Des_Ro_dis=51.523488 Action=0 S_N=36\n",
      "L60D270/300\n",
      "Des_Robot_a=-73.726799 Robot_a=45\n",
      "0.758484086512\n",
      "279655\n",
      "False_epoche=285\n",
      "**********************************\n",
      "last_x=899.992768, last_y=838.784369,last_a=90.000000\n",
      "x=899.992768 y=850.344369 True_angle=0.008345 Des_Ro_dis=49.655632 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=89.991655 Robot_a=90\n",
      "0.758141359291\n",
      "280113\n",
      "False_epoche=286\n",
      "**********************************\n",
      "last_x=851.290388, last_y=871.345694,last_a=45.000000\n",
      "x=859.464542 y=879.519848 True_angle=18.195249 Des_Ro_dis=45.415415 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=26.804751 Robot_a=45\n",
      "0.757869833262\n",
      "280476\n",
      "False_epoche=287\n",
      "**********************************\n",
      "last_x=842.491616, last_y=891.019316,last_a=0.000000\n",
      "x=854.051616 y=891.019316 True_angle=11.059133 Des_Ro_dis=46.817803 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=11.059133 Robot_a=0\n",
      "0.75753037742\n",
      "280930\n",
      "False_epoche=288\n",
      "**********************************\n",
      "last_x=849.000502, last_y=915.410321,last_a=0.000000\n",
      "x=860.560502 y=915.410321 True_angle=21.342238 Des_Ro_dis=42.343264 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=-21.342238 Robot_a=0\n",
      "0.757267293033\n",
      "281282\n",
      "False_epoche=289\n",
      "**********************************\n",
      "last_x=878.611273, last_y=848.582436,last_a=135.000000\n",
      "x=870.437118 y=856.756591 True_angle=79.358035 Des_Ro_dis=52.382787 Action=0 S_N=8\n",
      "L60D60/90\n",
      "Des_Robot_a=55.641965 Robot_a=135\n",
      "0.757148486979\n",
      "281441\n",
      "False_epoche=290\n",
      "**********************************\n",
      "last_x=861.230324, last_y=856.452525,last_a=90.000000\n",
      "x=861.230324 y=868.012525 True_angle=50.475178 Des_Ro_dis=50.262176 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=39.524822 Robot_a=90\n",
      "0.756880308813\n",
      "281800\n",
      "False_epoche=291\n",
      "**********************************\n",
      "last_x=868.860503, last_y=843.825407,last_a=45.000000\n",
      "x=877.034657 y=851.999562 True_angle=19.431670 Des_Ro_dis=53.211362 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=64.431670 Robot_a=45\n",
      "0.756658518299\n",
      "282097\n",
      "False_epoche=292\n",
      "**********************************\n",
      "last_x=883.887747, last_y=845.885991,last_a=90.000000\n",
      "x=883.887747 y=857.445991 True_angle=20.738203 Des_Ro_dis=45.502180 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=69.261797 Robot_a=90\n",
      "0.756430822176\n",
      "282402\n",
      "False_epoche=293\n",
      "**********************************\n",
      "last_x=898.138829, last_y=843.105318,last_a=90.000000\n",
      "x=898.138829 y=854.665318 True_angle=2.350902 Des_Ro_dis=45.372870 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=87.649098 Robot_a=90\n",
      "0.756118879293\n",
      "282820\n",
      "False_epoche=294\n",
      "**********************************\n",
      "last_x=895.869364, last_y=845.893691,last_a=90.000000\n",
      "x=895.869364 y=857.453691 True_angle=5.545219 Des_Ro_dis=42.746352 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=84.454781 Robot_a=90\n",
      "0.755273260041\n",
      "283954\n",
      "False_epoche=295\n",
      "**********************************\n",
      "last_x=864.572648, last_y=852.342067,last_a=45.000000\n",
      "x=872.746802 y=860.516222 True_angle=10.385094 Des_Ro_dis=47.976093 Action=0 S_N=44\n",
      "L60D330/360\n",
      "Des_Robot_a=55.385094 Robot_a=45\n",
      "0.75524419495\n",
      "283993\n",
      "False_epoche=296\n",
      "**********************************\n",
      "last_x=891.608267, last_y=841.130805,last_a=90.000000\n",
      "x=891.608267 y=852.690805 True_angle=10.058537 Des_Ro_dis=48.047697 Action=0 S_N=0\n",
      "L60D0/30\n",
      "Des_Robot_a=79.941463 Robot_a=90\n",
      "0.754830699178\n",
      "284548\n",
      "False_epoche=297\n",
      "**********************************\n",
      "last_x=898.024204, last_y=842.895429,last_a=45.000000\n",
      "x=906.198358 y=851.069584 True_angle=52.219603 Des_Ro_dis=49.321448 Action=0 S_N=40\n",
      "L60D300/330\n",
      "Des_Robot_a=-82.780397 Robot_a=45\n",
      "0.754598348248\n",
      "284860\n",
      "False_epoche=298\n",
      "**********************************\n",
      "last_x=869.353791, last_y=851.528990,last_a=90.000000\n",
      "x=869.353791 y=863.088990 True_angle=39.701928 Des_Ro_dis=47.975127 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=50.298072 Robot_a=90\n",
      "0.754294614088\n",
      "285268\n",
      "False_epoche=299\n",
      "**********************************\n",
      "last_x=856.807344, last_y=857.463107,last_a=90.000000\n",
      "x=856.807344 y=869.023107 True_angle=54.352679 Des_Ro_dis=53.152360 Action=0 S_N=4\n",
      "L60D30/60\n",
      "Des_Robot_a=35.647321 Robot_a=90\n",
      "0.754009603834\n",
      "285651\n",
      "False_epoche=300\n"
     ]
    }
   ],
   "source": [
    "def run2():\n",
    "    global Epoche\n",
    "    global Action_times\n",
    "    global Epoche_action_interval\n",
    "    global Vec_x\n",
    "    global Vec_y\n",
    "    Crash=False\n",
    "    Arrive=False\n",
    "    False_epoche=0\n",
    "    \n",
    "    Destination_x,Destination_y=Set_destination(900,900)\n",
    "    im=Draw_map(Destination_x,Destination_y)\n",
    "    \n",
    "    Destination_x,Destination_y=Set_destination(900,900)\n",
    "    Current_x,Current_y,Current_angle=Random_start(im)\n",
    "    Epoche_false=0\n",
    "    Vec_x=[]\n",
    "    Vec_y=[]\n",
    "    Vec_x.append(Current_x)\n",
    "    Vec_y.append(Current_y)\n",
    "    while False_epoche<300:\n",
    "        True_angle,Left_right,Distance=Robot_destination(Destination_x,Destination_y,\n",
    "                                                         Current_x,Current_y,Current_angle)\n",
    "        Current_state_number,Distance_level,Angle_level=Output_state_index(True_angle,\n",
    "                                                                           Left_right,Distance)\n",
    "        Next_action=Choose_action2(Q_Table2,Current_state_number,\n",
    "                                   Action_times)\n",
    "        Next_x,Next_y,Next_angle,Reward,Next_state_number,Arrive,Crash=Output_next_state(Current_x,Current_y,\n",
    "                                                                Current_angle,Destination_x,Destination_y,Next_action,im)\n",
    "        if Crash==True:\n",
    "            Next_x,Next_y,Next_angle=Random_start(im)\n",
    "        else:\n",
    "            if Arrive==True:\n",
    "                Next_true_angle,Next_left_right,Next_distance=Robot_destination(Destination_x,Destination_y,Next_x,Next_y,Next_angle)\n",
    "                print('**********************************')\n",
    "                print('last_x=%f, last_y=%f,last_a=%f'%(Current_x,Current_y,Current_angle%360))\n",
    "                print('x=%f y=%f True_angle=%f Des_Ro_dis=%f Action=%d S_N=%d'%(Next_x,Next_y,Next_true_angle,\n",
    "                                                                                Next_distance,Next_action,Next_state_number))\n",
    "                print(Q_table2_states[Next_state_number])\n",
    "                print('Des_Robot_a=%f Robot_a=%d'%\n",
    "                      ((np.arctan((Next_y-Destination_y)/(Next_x-Destination_x)))*180/np.pi,Next_angle%360))\n",
    "                print(Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_rate*Action_times))\n",
    "                print(Action_times)\n",
    "                Next_x,Next_y,Next_a=Random_start(im)\n",
    "                Epoche+=1\n",
    "                False_epoche+=1\n",
    "                print('False_epoche=%d'%False_epoche)\n",
    "            Q_target=Reward+Beta*max(Q_Table2[Next_state_number])\n",
    "            Q_Table2[Current_state_number][Next_action]+=Alpha*(Q_target-Q_Table2[Current_state_number][Next_action])\n",
    "            Vec_x.append(Current_x)\n",
    "            Vec_y.append(Current_y)\n",
    "        Current_x=Next_x\n",
    "        Current_y=Next_y\n",
    "        Current_angle=Next_angle\n",
    "        Action_times+=1\n",
    "    return Q_Table2\n",
    "Q_Table2_final=run2()\n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Q_Table2_=pd.DataFrame(Q_Table2_final,columns=Q_table2_action,index=Q_table2_states)\n",
    "Q_Table2_"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib qt5\n",
    "def Draw_result(Destination_x,Destination_y,Start_x,Start_y,Start_angle,Q_table):\n",
    "    Path_x=[]\n",
    "    Path_y=[]\n",
    "    Current_x=Start_x\n",
    "    Current_y=Start_y\n",
    "    Current_angle=Start_angle\n",
    "    Arrive=False\n",
    "    im=Draw_map(Destination_x,Destination_y)\n",
    "    while Arrive==False:\n",
    "        Path_x.append(Current_x)\n",
    "        Path_y.append(Current_y)\n",
    "        True_angle,Left_right,Distance=Robot_destination(Destination_x,Destination_y,\n",
    "                                                             Current_x,Current_y,Current_angle)\n",
    "        Current_state_number,Distance_level,Angle_level=Output_state_index(True_angle,\n",
    "                                                                               Left_right,Distance)\n",
    "        State_action=Q_Table2_final[Current_state_number,:]\n",
    "        Next_action=np.argmax(State_action)\n",
    "        Next_x,Next_y,Next_angle,Reward,Next_state_number,Arrive,Crash=Output_next_state(Current_x,Current_y,\n",
    "                                                                    Current_angle,Destination_x,Destination_y,Next_action,im)\n",
    "        Current_x=Next_x\n",
    "        Current_y=Next_y\n",
    "        Current_angle=Next_angle\n",
    "    im_result=Image.new(\"RGB\", size=(1000,1000),color=(0,0,0))\n",
    "    draw_result=ImageDraw.Draw(im_result,mode='RGB')\n",
    "    draw_result.rectangle((40,40,960,960),(255,255,255),(255,255,255))\n",
    "    draw_result.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20),(255,0,0),(255,0,0))\n",
    "    for i,j in zip(Path_x,Path_y):\n",
    "        draw_result.ellipse((i-20,j-20, i+20,j+20),(0,255,0),(0,255,0))\n",
    "    return im_result\n",
    "im_result=Draw_result(800,800,200,200,0,Q_Table2_final)\n",
    "plt.imshow(im_result)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# np.savetxt(\"Q_Table2_notgait2.txt\",Q_Table2_final)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
